12/18/23, 2:54 PM Extended and Unscented Kalman Filter Algorithms for Online State Estimation - MATLAB & Simulink

line State Estimation - MATLAB & Simulink - MathWorks Australia

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:

x[k + 1] = f ( x[k], w[k], us[k])

y[k] = h( x[k], v[k], um[k])

w[k] ~ (0, Q[k])

v[k] ~ (0, R[k])

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 us and um. 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:

1. Initialize the filter object with initial values of the state, x[0], and state estimation error covariance matrix, P.

x̂[0∣ − 1] = E ( x[0])

T
P[0∣ − 1] = E ( x[0] − x̂[0∣ − 1])( x[0] − x̂[0∣ − 1])

Here x̂ is the state estimate and x̂[ka∣kb]​ denotes the state estimate at time step ka using measurements at time steps 0,1,...,kb. So 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.

https://au.mathworks.com/help/control/ug/extended-and-unscented-kalman-filter-algorithms-for-online-state-estimation.html 1/8
12/18/23, 2:54 PM Extended and Unscented Kalman Filter Algorithms for Online State Estimation - MATLAB & Simulink - MathWorks Australia
2. For time steps k = 0,1,2,3,..., perform the following:

a. 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.

∂h ∣
C [k] =
∣
∂x
x̂[k ∣k − 1]

∂h ∣
S[k] =
∣
∂v
x̂[k ∣k − 1]

The software calculates these Jacobian matrices numerically unless you specify your own function handle for the Jacobian.

−1
T T T
K [k] = P[k ∣k − 1]C [k] (C [k]P[k ∣k − 1]C [k] + S[k] R[k]S[k] )

x̂[k ∣k] = x̂[k ∣k − 1] + K [k]( y[k] − h( x̂[k ∣k − 1], 0, um[k])

ˆ
P[k ∣k] = P[k ∣k − 1] − K [k] C [k] P[k ∣k − 1]

Here K is the Kalman gain.

b. 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.

∂ f ∣
A[k] =
∣
∂x
x̂[k ∣k]

∂ f ∣
G[k] =
∣
∂w
x̂[k ∣k]

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.

T T
P[k + 1∣k] = A[k]P[k ∣k] A[k] + G[k]Q[k]G[k]

x̂[k + 1∣k] = f ( x̂[k ∣k], 0, us[k])

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:

https://au.mathworks.com/help/control/ug/extended-and-unscented-kalman-filter-algorithms-for-online-state-estimation.html 2/8
12/18/23, 2:54 PM Extended and Unscented Kalman Filter Algorithms for Online State Estimation - MATLAB & Simulink - MathWorks Australia

If the process noise w is additive, that is the state transition equation has the form x[k] = f ( x[k − 1], us[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], um[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:

x[k + 1] = f ( x[k], us[k]) + w[k]

y[k] = h( x[k], um[k]) + v[k]

w[k] ~ (0, Q[k])

v[k] ~ (0, R[k])

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

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:

1. Initialize the filter object with initial values of the state, x[0], and state estimation error covariance, P.

x̂[0∣ − 1] = E ( x[0])

T
P[0∣ − 1] = E ( x[0] − x̂[0∣ − 1])( x[0] − x̂[0∣ − 1])

Here x̂ is the state estimate and x̂[ka∣kb]​ denotes the state estimate at time step ka using measurements at time steps 0,1,...,kb. So 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.

2. 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.

a. Choose the sigma points x̂

(i)
[k ∣k − 1] at time step k.

https://au.mathworks.com/help/control/ug/extended-and-unscented-kalman-filter-algorithms-for-online-state-estimation.html 3/8
12/18/23, 2:54 PM Extended and Unscented Kalman Filter Algorithms for Online State Estimation - MATLAB & Simulink - MathWorks Australia
(0)
x̂ [k ∣k − 1] = x̂[k ∣k − 1]

(i) (i)
x̂ [k ∣k − 1] = x̂[k ∣k − 1] + Δ x i = 1, ..., 2M

(i)
Δx = (√cP[k ∣k − 1])i i = 1, ..., M

(M + i)
Δx = − (√cP[k ∣k − 1]) i = 1, ..., M
i

Where c = α (M + κ)
2
is a scaling factor based on number of states M, and the parameters α and κ. For more information about the parameters, see Effect of Alpha,
T

Beta, and Kappa Parameters. √cP is the matrix square root of cP such that √cP (√cP) = cP and (√cP)i is the ith column of √cP .

b. Use the nonlinear measurement function to compute the predicted measurements for each of the sigma points.

(i) (i)
ŷ [k ∣k − 1] = h( x̂ [k ∣k − 1], um[k]) i = 0, 1, ..., 2M

c. Combine the predicted measurements to obtain the predicted measurement at time k.

2M

(i) (i)
ŷ[k] = W ŷ [k ∣k − 1]
∑ M
i=0

(0) M
W = 1 −
M 2
α (M + κ)

i 1
W = i = 1, 2, ..., 2M
M 2
2α ( M + κ )

d. Estimate the covariance of the predicted measurement. Add R[k] to account for the additive measurement noise.

2M

T
(i) (i) (i)
Py = W (ŷ [k ∣k − 1] − ŷ[k])( ŷ [k ∣k − 1] − ŷ[k]) + R[k]
∑ c
i=0

(0) 2 M
W = (2 − α + β) −
c 2
α (M + κ)

i 2
W = 1/(2α ( M + κ )) i = 1, 2, ..., 2M
c

For information about β parameter, see Effect of Alpha, Beta, and Kappa Parameters.

e. Estimate the cross-covariance between x̂[k ∣k − 1] and ŷ[k] .

2M

1 (i) (i)
T
P xy = ( x̂ [k ∣k − 1] − x̂[k ∣k − 1])( ŷ [k ∣k − 1] − ŷ[k])
2 ∑
2α (m + κ ) i=1

https://au.mathworks.com/help/control/ug/extended-and-unscented-kalman-filter-algorithms-for-online-state-estimation.html 4/8
12/18/23, 2:54 PM Extended and Unscented Kalman Filter Algorithms for Online State Estimation - MATLAB & Simulink - MathWorks Australia

The summation starts from i = 1 because x̂

(0)
[k ∣k − 1] − x̂[k ∣k − 1] = 0 .
f. Obtain the estimated state and state estimation error covariance at time step k.

−1
K = P xyP
y

x̂[k ∣k] = x̂[k ∣k − 1] + K ( y[k] − ŷ[k])

T
P[k ∣k] = P[k ∣k − 1] − K P yK
k

Here K is the Kalman gain.

3. Predict the state and state estimation error covariance at the next time step. In the software, the predict command performs this prediction.

a. Choose the sigma points x̂

(i)
[k ∣k] at time step k.

(0)
x̂ [k ∣k] = x̂[k ∣k]

(i) (i)
x̂ [k ∣k] = x̂[k ∣k] + Δ x i = 1, ..., 2M

(i)
Δx = (√cP[k ∣k])i i = 1, ..., M

(M + i)
Δx = − (√cP[k ∣k]) i = 1, ..., M
i

b. Use the nonlinear state transition function to compute the predicted states for each of the sigma points.

(i) (i)
x̂ [k + 1∣k] = f ( x̂ [k ∣k], us[k])

c. 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.

2M

(i) (i)
x̂[k + 1∣k] = W x̂ [k + 1∣k]
∑ M
i=0

(0) M
W = 1 −
M 2
α (M + κ)

i 1
W = i = 1, 2, ..., 2M
M 2
2α ( M + κ )

d. 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.

https://au.mathworks.com/help/control/ug/extended-and-unscented-kalman-filter-algorithms-for-online-state-estimation.html 5/8
12/18/23, 2:54 PM Extended and Unscented Kalman Filter Algorithms for Online State Estimation - MATLAB & Simulink - MathWorks Australia
2M

T
(i) (i) (i)
P[k + 1∣k] = W ( x̂ [k + 1∣k] − x̂[k + 1∣k])( x̂ [k + 1∣k] − x̂[k + 1∣k]) + Q[k]
∑ c
i=0

(0) 2 M
W = (2 − α + β) −
c 2
α (M + κ)

i 2
W = 1/(2α ( M + κ )) i = 1, 2, ..., 2M
c

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.

https://au.mathworks.com/help/control/ug/extended-and-unscented-kalman-filter-algorithms-for-online-state-estimation.html 6/8
12/18/23, 2:54 PM Extended and Unscented Kalman Filter Algorithms for Online State Estimation - MATLAB & Simulink - MathWorks Australia

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.

See Also

Functions
extendedKalmanFilter | unscentedKalmanFilter

Blocks
Extended Kalman Filter | Unscented Kalman Filter

External Websites
Understanding Kalman Filters: Nonlinear State Estimators — MATLAB Video Series

https://au.mathworks.com/help/control/ug/extended-and-unscented-kalman-filter-algorithms-for-online-state-estimation.html 7/8
12/18/23, 2:54 PM Extended and Unscented Kalman Filter Algorithms for Online State Estimation - MATLAB & Simulink - MathWorks Australia

https://au.mathworks.com/help/control/ug/extended-and-unscented-kalman-filter-algorithms-for-online-state-estimation.html 8/8