Kalman Filter

Predict or estimate states of dynamic systems

Library

Filtering/Adaptive Filters

dspadpt3

• 

Description

Use the Kalman Filter block to predict or estimate the state of a dynamic system from a series of incomplete and/or noisy measurements. Suppose you have a noisy linear system that is defined by the following equations:

$$\begin{array}{l}{x}_k=A{x}_{k-1}+w_{k-1}\\ z_k=H{x}_k+v_k\end{array}$$

This block can use the previously estimated state, $${\widehat{x}}_{k-1}$$, to predict the current state at time k, $$x_k^{-}$$, as shown by the following equation:

$$\begin{array}{l}{x}_k^{-}=A{\widehat{x}}_{k-1}\\ P_k^{-}=A{\widehat{P}}_{k-1}{A}^T+Q\end{array}$$

The block can also use the current measurement, $$z_k$$, and the predicted state, $$x_k^{-}$$, to estimate the current state value at time k, $${\widehat{x}}_k$$, so that it is a more accurate approximation:

$$\begin{array}{l}{K}_k=P_k^{-}{H}^T{\left(H{P}_k^{-}{H}^T+R\right)}^{-1}\\ {\widehat{x}}_k=x_k^{-}+K_k\left(z_k-H{x}_k^{-}\right)\\ {\widehat{P}}_k=\left(I-K_{k}H\right)P_k^{-}\end{array}$$

The variables in the previous equations are defined in the following table.

Variable	Definition	Default Value or Initial Condition
x	State	N/A
$$\widehat{x}$$	Estimated state	zeros([6, 1])
$$x^{-}$$	Predicted state	N/A
A	State transition matrix	$$\left[\begin{array}{cccccc}1& 0& 1& 0& 0& 0\\ 0& 1& 0& 1& 0& 0\\ 0& 0& 1& 0& 0& 0\\ 0& 0& 0& 1& 0& 0\\ 0& 0& 0& 0& 1& 0\\ 0& 0& 0& 0& 0& 1\end{array}\right]$$
w	Process noise	N/A
z	Measurement	N/A
H	Measurement matrix	$$\left[\begin{array}{cccccc}1& 0& 0& 0& 0& 0\\ 0& 1& 0& 0& 0& 0\\ 0& 0& 0& 0& 1& 0\\ 0& 0& 0& 0& 0& 1\end{array}\right]$$
v	Measurement noise	N/A
$$\widehat{P}$$	Estimated error covariance	10*eye(6)
P-	Predicted error covariance	N/A
Q	Process noise covariance	0.05*eye(6)
K	Kalman gain	N/A
R	Measurement noise covariance	eye(4)
I	Identity matrix	N/A

In the previous equations, z is a vector of measurement values. Most of the time, the block processes Z, an M-by-N matrix, where M is the number of measurement values and N is the number of filters.

Use the Number of filters parameter to specify the number of filters to use to predict or estimate the current value.

Use the Enable filters parameter to specify which filters are enabled or disabled at each time step. If you select Always, the filters are always enabled. If you choose Specify via input port <Enable>, the Enable port appears on the block. The input to this port must be a row vector of 1s and 0s whose length is equal to the number of filters. For example, if there are 3 filters and the input to the Enable port is [1 0 1], only the first and third filter are enabled at this time step. If you select the Reset the estimated state and estimated error covariance when filters are disabled check box, the estimated and predicted states as well as the estimated error covariance that correspond to the disabled filters are reset to their initial values.

Note

All filters have the same state transition matrix, measurement matrix, initial conditions, and noise covariance, but their state, measurement, enable, and MSE signals are unique. Within the state, measurement, enable, and MSE signals, each column corresponds to a filter.

Use the Measurement matrix source parameter to specify how to enter the measurement matrix values. If you select Specify via dialog, the Measurement matrix parameter appears in the dialog box. If you select Input port <H>, the H port appears on the block. Use this port to specify your measurement matrix.

Parameters

Number of filters

Specify the number of filters to use to predict or estimate the current value.

Enable filters

Specify which filters are enabled or disabled at each time step. If you select Always, the filters are always enabled. If you choose Specify via input port <Enable>, the Enable port appears on the block.

Reset the estimated state and estimated error covariance when filters are disabled

If you select this check box, the estimated and predicted states as well as the estimated error covariance that correspond to the disabled filters are reset to their initial values. This parameter is visible if, for the Enable filters parameter, you select Specify via input port <Enable>.

Initial condition for estimated state

Enter the initial condition for the estimated state.

Initial condition for estimated error covariance

Enter the initial condition for the estimated error covariance.

State transition matrix

Enter the state transition matrix.

Process noise covariance

Enter the process noise covariance.

Measurement matrix source

Specify how to enter the measurement matrix values. If you select Specify via dialog, the Measurement matrix parameter appears in the dialog box. If you select Input port <H>, the H port appears on the block.

Measurement matrix

Enter the measurement matrix values. This parameter is visible if you select Specify via dialog for the Measurement matrix source parameter.

Measurement noise covariance

Enter the measurement noise covariance.

Output estimated measurement <Z_est>

Select this check box if you want the block to output the estimated measurement.

Output estimated state <X_est>

Select this check box if you want the block to output the estimated state.

Output MSE of estimated state <MSE_est>

Select this check box if you want the block to output the mean-squared error of the estimated state.

Output predicted measurement <Z_prd>

Select this check box if you want the block to output the predicted measurement.

Output predicted state <X_prd>

Select this check box if you want the block to output the predicted state.

Output MSE of predicted state <MSE_prb>

Select this check box if you want the block to output the mean-squared error of the predicted state.

References

[1] Haykin, Simon. Adaptive Filter Theory. Upper Saddle River, NJ: Prentice Hall, 1996.

[2] Welch, Greg and Gary Bishop, “An Introduction to the Kalman Filter,” TR 95–041, Department of Computer Science, University of North Carolina.

Supported Data Types

Port	Input/Output	Supported Data Types
Z	M-by-N measurement where M is the length of the measurement vector and N is the number of filters.	Double-precision floating point Single-precision floating point
Enable	1-by-N vector of 1s and 0s where N is the number of filters.	Double-precision floating point Single-precision floating point Boolean
H	M-by-P measurement matrix where M is the length of the measurement vector and P is the length of the filter state vectors.	Same as Z port
Z_est	M-by-N estimated measurement matrix where M is the length of the measurement vector and N is the number of filters.	Same as Z port
X_est	P-by-N estimated state matrix where P is the length of the filter state vectors and N is the number of filters.	Same as Z port
MSE_est	1-by-N vector that represents the mean-squared-error of the estimated state. N is the number of filters.	Same as Z port
Z_prd	M-by-N predicted measurement matrix where M is the length of the measurement vector and N is the number of filters.	Same as Z port
X_prd	P-by-N predicted state matrix where P is the length of the filter state vectors and N is the number of filters.	Same as Z port
MSE_prd	1-by-N vector that represents the mean-squared-error of the predicted state. N is the number of filters.	Same as Z port