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}_{k1}+{w}_{k1}\\ {z}_{k}=H{x}_{k}+{v}_{k}\end{array}$$
This block can use the previously estimated state, $${\widehat{x}}_{k1}$$, to predict the current state at time k, $${x}_{k}^{}$$, as shown by the following equation:
$$\begin{array}{l}{x}_{k}^{}=A{\widehat{x}}_{k1}\\ {P}_{k}^{}=A{\widehat{P}}_{k1}{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 MbyN 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 chooseSpecify 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 selectInput 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 meansquared 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 meansquared 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  MbyN measurement where M is the length of the measurement vector and N is the number of filters. 

Enable  1byN vector of 1s and 0s where N is the number of filters. 

H  MbyP 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  MbyN 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  PbyN 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  1byN vector that represents the meansquarederror of the estimated state. N is the number of filters.  Same as Z port 
Z_prd  MbyN 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  PbyN 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  1byN vector that represents the meansquarederror of the predicted state. N is the number of filters.  Same as Z port 
See Also
LDL Solver  DSP System Toolbox 