Extended and Unscented Kalman Filter Algorithms for Online State Estimation

You can use discrete-time extended and unscented Kalman filter algorithms for online state estimation of discrete-time nonlinear systems. If you have a system with severe nonlinearities, the unscented Kalman filter algorithm may give better estimation results. You can perform the state estimation in Simulink^® and at the command line. To perform the state estimation, you first create the nonlinear state transition function and measurement function for your system.

At the command line, you use the functions to construct the extendedKalmanFilter or unscentedKalmanFilter object for desired algorithm, and specify whether the process and measurement noise terms in the functions are additive or non-additive. After you create the object, you use the predict and correct commands to estimate the states using real-time data. For information about the order in which to execute these commands, see the predict and correct reference pages.

In Simulink, you specify these function in the Extended Kalman Filter and Unscented Kalman Filter blocks. You also specify whether the process and measurement noise terms in the functions are additive or non-additive. In the blocks, the software decides the order in which prediction and correction of state estimates is done.

Extended Kalman Filter Algorithm

The extendedKalmanFilter command and Extended Kalman Filter block implement the first-order discrete-time Kalman filter algorithm. Assume that the state transition and measurement equations for a discrete-time nonlinear system have non-additive process and measurement noise terms with zero mean and covariance matrices Q and R, respectively:

$\begin{array}{l} x [k + 1] = f (x [k], w [k], u_{s} [k]) \\ y [k] = h (x [k], v [k], u_{m} [k]) \\ w [k] ~ (0, Q [k]) \\ v [k] ~ (0, R [k]) \end{array}$

Here f is a nonlinear state transition function that describes the evolution of states x from one time step to the next. The nonlinear measurement function h relates x to the measurements y at time step k. These functions can also have additional input arguments that are denoted by u_s and u_m. The process and measurement noise are w and v, respectively. You provide Q and R.

In the block, the software decides the order of prediction and correction of state estimates. At the command line, you decide the order. For information about the order in which to execute these commands, see the predict and correct reference pages. Assuming that you implement the correct command before predict, the software implements the algorithm as follows:

Initialize the filter object with initial values of the state, x[0], and state estimation error covariance matrix, P.
$\begin{array}{l} \hat{x} [0 | - 1] = E (x [0]) \\ P [0 | - 1] = E (x [0] - \hat{x} [0 | - 1]) (^{x [0] - \hat{x} [0 | - 1]) T} \end{array}$
Here $\hat{x}$ is the state estimate and $\hat{x} [k_{a} | k_{b}] $ denotes the state estimate at time step k_a using measurements at time steps 0,1,...,k_b. So $\hat{x} [0 | - 1]$ is the best guess of the state value before you make any measurements. You specify this value when you construct the filter.
For time steps k = 0,1,2,3,..., perform the following:
1. Compute the Jacobian of the measurement function, and update the state and state estimation error covariance using the measured data, y[k]. At the command line, the correct command performs this update.
  $\begin{array}{l} C [k] = {\frac{\partial h}{\partial x} |}_{\hat{x} [k | k - 1]} \\ S [k] = {\frac{\partial h}{\partial v} |}_{\hat{x} [k | k - 1]} \end{array}$
  The software calculates these Jacobian matrices numerically unless you specify your own function handle for the Jacobian.
  $\begin{array}{l} K [k] = P [k | k - 1] C [^{k] T} (^{C [k] P [k | k - 1] C [^{k] T} + S [k] R [k] S [^{k] T}) - 1} \\ \hat{x} [k | k] = \hat{x} [k | k - 1] + K [k] (y [k] - h (\hat{x} [k | k - 1], 0, u_{m} [k]) \\ \hat{P} [k | k] = P [k | k - 1] - K [k] C [k] P [k | k - 1] \end{array}$
  Here K is the Kalman gain.
2. Compute the Jacobian of the state transition function, and predict the state and state estimation error covariance at the next time step. In the software, the predict command performs this prediction.
  $\begin{array}{l} A [k] = {\frac{\partial f}{\partial x} |}_{\hat{x} [k | k]} \\ G [k] = {\frac{\partial f}{\partial w} |}_{\hat{x} [k | k]} \end{array}$
  The software calculates these Jacobian matrices numerically unless you specify the analytical Jacobian. This numerical computation may increase processing time and numerical inaccuracy of the state estimation.
  $\begin{array}{l} P [k + 1 | k] = A [k] P [k | k] A [^{k] T} + G [k] Q [k] G [^{k] T} \\ \hat{x} [k + 1 | k] = f (\hat{x} [k | k], 0, u_{s} [k]) \end{array}$
  The correct function uses these values in the next time step. For better numerical performance, the software uses the square-root factorization of the covariance matrices. For more information on this factorization, see [2].

The Extended Kalman Filter block supports multiple measurement functions. These measurements can have different sample times as long as their sample time is an integer multiple of the state transition sample time. In this case, a separate correction step is performed corresponding to measurements from each measurement function.

The algorithm steps described previously assume that you have non-additive noise terms in the state transition and measurement functions. If you have additive noise terms in the functions, the changes in the algorithm are:

If the process noise w is additive, that is the state transition equation has the form $x [k] = f (x [k - 1], u_{s} [k - 1]) + w [k - 1]$ , then the Jacobian matrix G[k] is an identity matrix.
If the measurement noise v is additive, that is the measurement equation has the form $y [k] = h (x [k], u_{m} [k]) + v [k]$ , then the Jacobian matrix S[k] is an identity matrix.

Additive noise terms in the state and transition functions reduce the processing time.

The first-order extended Kalman filter uses linear approximations to nonlinear state transition and measurement functions. As a result, the algorithm may not be reliable if the nonlinearities in your system are severe. The unscented Kalman filter algorithm may yield better results in this case.

Unscented Kalman Filter Algorithm

The unscented Kalman filter algorithm and Unscented Kalman Filter block use the unscented transformation to capture the propagation of the statistical properties of state estimates through nonlinear functions. The algorithm first generates a set of state values called sigma points. These sigma points capture the mean and covariance of the state estimates. The algorithm uses each of the sigma points as an input to the state transition and measurement functions to get a new set of transformed state points. The mean and covariance of the transformed points is then used to obtain state estimates and state estimation error covariance.

Assume that the state transition and measurement equations for an M-state discrete-time nonlinear system have additive process and measurement noise terms with zero mean and covariances Q and R, respectively:

$\begin{array}{l} x [k + 1] = f (x [k], u_{s} [k]) + w [k] \\ y [k] = h (x [k], u_{m} [k]) + v [k] \\ w [k] ~ (0, Q [k]) \\ v [k] ~ (0, R [k]) \end{array}$

You provide the initial values of Q and R in the ProcessNoise and MeasurementNoise properties of the unscented Kalman filter object.

Initialize the filter object with initial values of the state, x[0], and state estimation error covariance, P.
$\begin{array}{l} \hat{x} [0 | - 1] = E (x [0]) \\ P [0 | - 1] = E (x [0] - \hat{x} [0 | - 1]) (^{x [0] - \hat{x} [0 | - 1]) T} \end{array}$
Here $\hat{x}$ is the state estimate and $\hat{x} [k_{a} | k_{b}] $ denotes the state estimate at time step k_a using measurements at time steps 0,1,...,k_b. So $\hat{x} [0 | - 1]$ is the best guess of the state value before you make any measurements. You specify this value when you construct the filter.
For each time step k, update the state and state estimation error covariance using the measured data, y[k]. In the software, the correct command performs this update.
1. Choose the sigma points ${\hat{x}}^{(i)} [k | k - 1]$ at time step k.
  $\begin{array}{l} {\hat{x}}^{(0)} [k | k - 1] = \hat{x} [k | k - 1] \\ {\hat{x}}^{(i)} [k | k - 1] = \hat{x} [k | k - 1] + Δ x^{(i)} i = 1, ..., 2 M \\ Δ x^{(i)} = (\sqrt{c P [k | k - 1]}) {}_{i} i = 1, ..., M \\ Δ x^{(M + i)} = - {(\sqrt{c P [k | k - 1]})}_{i} i = 1, ..., M \end{array}$
  Where $c = α^{2} (M + κ)$ is a scaling factor based on number of states M, and the parameters α and κ. For more information about the parameters, see Effect of Alpha, Beta, and Kappa Parameters. $\sqrt{c P}$ is the matrix square root of cP such that $\sqrt{c P} {(\sqrt{c P})}^{T} = c P$ and ${(\sqrt{c P})}_{i}$ is the ith column of $\sqrt{c P}$ .
2. Use the nonlinear measurement function to compute the predicted measurements for each of the sigma points.
  ${\hat{y}}^{(i)} [k | k - 1] = h ({\hat{x}}^{(i)} [k | k - 1], u_{m} [k]) i = 0, 1, ..., 2 M$
3. Combine the predicted measurements to obtain the predicted measurement at time k.
  $\begin{array}{l} \hat{y} [k] = \sum_{i = 0}^{2 M} W_{M}^{(i)} {\hat{y}}^{(i)} [k | k - 1] \\ W_{M}^{(0)} = 1 - \frac{M}{α^{2} (M + κ)} \\ W_{M}^{i} = \frac{1}{2 α^{2} (M + κ)} i = 1, 2, ..., 2 M \end{array}$
4. Estimate the covariance of the predicted measurement. Add R[k] to account for the additive measurement noise.
  $\begin{array}{l} P_{y} = \sum_{i = 0}^{2 M} W_{c}^{(i)} ({\hat{y}}^{(i)} [k | k - 1] - \hat{y} [k]) (^{{\hat{y}}^{(i)} [k | k - 1] - \hat{y} [k]) T} + R [k] \\ W_{c}^{(0)} = (2 - α^{2} + β) - \frac{M}{α^{2} (M + κ)} \\ W_{c}^{i} = 1 / (2 α^{2} (M + κ)) i = 1, 2, ..., 2 M \end{array}$
  For information about β parameter, see Effect of Alpha, Beta, and Kappa Parameters.
5. Estimate the cross-covariance between $\hat{x} [k | k - 1]$ and $\hat{y} [k]$ .
  $P_{x y} = \frac{1}{2 α^{2} (m + κ)} \sum_{i = 1}^{2 M} ({\hat{x}}^{(i)} [k | k - 1] - \hat{x} [k | k - 1]) {({\hat{y}}^{(i)} [k | k - 1] - \hat{y} [k])}^{T}$
  The summation starts from i = 1 because ${\hat{x}}^{(0)} [k | k - 1] - \hat{x} [k | k - 1] = 0$ .
6. Obtain the estimated state and state estimation error covariance at time step k.
  $\begin{array}{l} K = P_{x y} P_{y}^{- 1} \\ \hat{x} [k | k] = \hat{x} [k | k - 1] + K (y [k] - \hat{y} [k]) \\ P [k | k] = P [k | k - 1] - K P_{y} K_{k}^{T} \end{array}$
  Here K is the Kalman gain.
Predict the state and state estimation error covariance at the next time step. In the software, the predict command performs this prediction.
1. Choose the sigma points ${\hat{x}}^{(i)} [k | k]$ at time step k.
  $\begin{array}{l} {\hat{x}}^{(0)} [k | k] = \hat{x} [k | k] \\ {\hat{x}}^{(i)} [k | k] = \hat{x} [k | k] + Δ x^{(i)} i = 1, ..., 2 M \\ Δ x^{(i)} = (\sqrt{c P [k | k]}) {}_{i} i = 1, ..., M \\ Δ x^{(M + i)} = - {(\sqrt{c P [k | k]})}_{i} i = 1, ..., M \end{array}$
2. Use the nonlinear state transition function to compute the predicted states for each of the sigma points.
  ${\hat{x}}^{(i)} [k + 1 | k] = f ({\hat{x}}^{(i)} [k | k], u_{s} [k])$
3. Combine the predicted states to obtain the predicted states at time k+1. These values are used by the correct command in the next time step.
  $\begin{array}{l} \hat{x} [k + 1 | k] = \sum_{i = 0}^{2 M} W_{M}^{(i)} {\hat{x}}^{(i)} [k + 1 | k] \\ W_{M}^{(0)} = 1 - \frac{M}{α^{2} (M + κ)} \\ W_{M}^{i} = \frac{1}{2 α^{2} (M + κ)} i = 1, 2, ..., 2 M \end{array}$
4. Compute the covariance of the predicted state. Add Q[k] to account for the additive process noise. These values are used by the correct command in the next time step.
  
  $\begin{array}{l} P [k + 1 | k] = \sum_{i = 0}^{2 M} W_{c}^{(i)} ({\hat{x}}^{(i)} [k + 1 | k] - \hat{x} [k + 1 | k]) {({\hat{x}}^{(i)} [k + 1 | k] - \hat{x} [k + 1 | k])}^{T} + Q [k] \\ W_{c}^{(0)} = (2 - α^{2} + β) - \frac{M}{α^{2} (M + κ)} \\ W_{c}^{i} = 1 / (2 α^{2} (M + κ)) i = 1, 2, ..., 2 M \end{array}$

The Unscented Kalman Filter block supports multiple measurement functions. These measurements can have different sample times as long as their sample time is an integer multiple of the state transition sample time. In this case, a separate correction step is performed corresponding to measurements from each measurement function.

The previous algorithm is implemented assuming additive noise terms in the state transition and measurement equations. For better numerical performance, the software uses the square-root factorization of the covariance matrices. For more information on this factorization, see [2].

If the noise terms are non-additive, the main changes to the algorithm are:

The correct command generates 2*(M+V)+1 sigma points using P[k|k-1] and R[k], where V is the number of elements in measurement noise v[k]. The R[k] term is no longer added in the algorithm step 2(d) because the extra sigma points capture the impact of measurement noise on Py.
The predict command generates 2*(M+W)+1 sigma points using P[k|k] and Q[k], where W is the number of elements in process noise w[k]. The Q[k] term is no longer added in the algorithm step 3(d) because the extra sigma points capture the impact of process noise on P[k+1|k].

Effect of Alpha, Beta, and Kappa Parameters

To compute the state and its statistical properties at the next time step, the unscented Kalman filter algorithm generates a set of state values distributed around the mean state value. The algorithm uses each sigma points as an input to the state transition and measurement functions to get a new set of transformed state points. The mean and covariance of the transformed points is then used to obtain state estimates and state estimation error covariance.

The spread of the sigma points around the mean state value is controlled by two parameters α and κ. A third parameter, β, impacts the weights of the transformed points during state and measurement covariance calculations.

α — Determines the spread of the sigma points around the mean state value. It is usually a small positive value. The spread of sigma points is proportional to α. Smaller values correspond to sigma points closer to the mean state.
κ — A second scaling parameter that is usually set to 0. Smaller values correspond to sigma points closer to the mean state. The spread is proportional to the square-root of κ.
β — Incorporates prior knowledge of the distribution of the state. For Gaussian distributions, β = 2 is optimal.

You specify these parameters in the Alpha, Kappa, and Beta properties of the unscented Kalman filter. If you know the distribution of state and state covariance, you can adjust these parameters to capture the transformation of higher-order moments of the distribution. The algorithm can track only a single peak in the probability distribution of the state. If there are multiple peaks in the state distribution of your system, you can adjust these parameters so that the sigma points stay around a single peak. For example, choose a small Alpha to generate sigma points close to the mean state value.

References

[1] Simon, Dan. Optimal State Estimation: Kalman, H Infinity, and Nonlinear Approaches. Hoboken, NJ: John Wiley and Sons, 2006.

[2] Van der Merwe, Rudolph, and Eric A. Wan. “The Square-Root Unscented Kalman Filter for State and Parameter-Estimation.” 2001 IEEE International Conference on Acoustics, Speech, and Signal Processing. Proceedings (Cat. No.01CH37221), 6:3461–64. Salt Lake City, UT, USA: IEEE, 2001. https://doi.org/10.1109/ICASSP.2001.940586.

External Websites

Understanding Kalman Filters: Nonlinear State Estimators — MATLAB Video Series