Kalman Filter

Estimate states of discrete-time or continuous-time linear system

Description

Use the Kalman Filter block to estimate states of a state-space plant model given process and measurement noise covariance data. The state-space model can be time-varying. A steady-state Kalman filter implementation is used if the state-space model and the noise covariance matrices are all time-invariant. A time-varying Kalman filter is used otherwise.

Kalman filter provides the optimal solution to the following continuous or discrete estimation problems:

Continuous-Time Estimation

Given the continuous plant

$\begin{array}{l} \dot{x} (t) = A (t) x (t) + B (t) u (t) + G (t) w (t) (state equation) \\ y (t) = C (t) x (t) + D (t) u (t) + H (t) w (t) + v (t) (measurement equation) \end{array}$

with known inputs u, white process noise w, and white measurement noise v satisfying:

$\begin{array}{l} E [w (t)] = E [v (t)] = 0 \\ E [w (t) w^{T} (t)] = Q (t) \\ E [w (t) v^{T} (t)] = N (t) \\ E [v (t) v (^{t) T}] = R (t) \end{array}$

construct a state estimate $\hat{x}$ that minimizes the state estimation error covariance $P (t) = E [(x - \hat{x}) {(x - \hat{x})}^{T}]$ .

The optimal solution is the Kalman filter with equations

$\begin{array}{l} L (t) = (P (t) C^{T} (t) + \bar{N}), \\ \dot{P} (t) = A (t) P (t) + P (t) A^{T} (t) + \bar{Q} (t) - L (t) \bar{R} (t) L^{T} (t), \\ \dot{\hat{x}} (t) = A (t) \hat{x} (t) + B (t) u (t) + L (t) (y (t) - C (t) \hat{x} (t) - D (t) u (t)), \end{array}$

where

$\begin{array}{l} \bar{Q} (t) = G (t) Q (t) G^{T} (t), \\ \bar{R} (t) = R (t) + H (t) N (t) + N^{T} (t) H^{T} (t) + H (t) Q (t) H^{T} (t), \\ \bar{N} (t) = G (t) (Q (t) H^{T} (t) + N (t)) . \end{array}$

The Kalman filter uses the known inputs u and the measurements y to generate the state estimates $\hat{x}$ . If you want, the block can also output the estimates of the true plant output $\hat{y}$ .

The block implements the steady-state Kalman filter when the system matrices (A(t), B(t), C(t), D(t), G(t), H(t)) and noise covariance matrices (Q(t), R(t), N(t)) are constant (specified in the Block Parameters dialog box). The steady-state Kalman filter uses a constant matrix P that minimizes the steady-state estimation error covariance and solves the associated continuous-time algebraic Riccati equation:

$P = \lim_{t \to \infty} E [(x - \hat{x}) {(x - \hat{x})}^{T}] .$

Discrete-Time Estimation

Given the discrete plant

$\begin{array}{l} x [n + 1] = A [n] x [n] + B [n] u [n] + G [n] w [n], \\ y [n] = C [n] x [n] + D [n] u [n] + H [n] w [n] + v [n], \end{array}$

with known inputs u, white process noise w and white measurement noise v satisfying

$\begin{array}{l} E [w [n]] = E [v [n]] = 0, \\ E [w [n] w^{T} [n]] = Q [n], \\ E [v [n] v^{T} [n]] = R [n], \\ E [w [n] v^{T} [n]] = N [n] . \end{array}$

The estimator has the following state equation

$\hat{x} [n + 1 | n] = A [n] \hat{x} [n | n - 1] + B [n] u [n] + L [n] (y [n] - C [n] \hat{x} [n | n - 1] - D [n] u [n]),$

where the gain L[n] is calculated through the discrete Riccati equation:

$\begin{array}{l} L [n] = (A [n] P [n] C^{T} [n] + \bar{N} [n]) (^{C [n] P [n] C^{T} [n] + \bar{R} [n]) - 1}, \\ M [n] = P [n] C^{T} [n] (^{C [n] P [n] C^{T} [n] + \bar{R} [n]) - 1}, \\ Z [n] = (I - M [n] C [n]) P [n] (^{I - M [n] C [n]) T} + M [n] \bar{R} [n] M^{T} [n], \\ P [n + 1] = (A [n] - \bar{N} [n] {\bar{R}}^{- 1} [n] C [n]) Z (^{A [n] - \bar{N} [n] {\bar{R}}^{- 1} [n] C [n]) T} + \bar{Q} [n] - N [n] {\bar{R}}^{- 1} [n] N^{T} [n], \end{array}$

where I is the identity matrix of appropriate size and

$\begin{array}{l} \bar{Q} [n] = G [n] Q [n] G^{T} [n], \\ \bar{R} [n] = R [n] + H [n] N [n] + N^{T} [n] H^{T} [n] + H [n] Q [n] H^{T} [n], \\ \bar{N} [n] = G [n] (Q [n] H^{T} [n] + N [n]), \\ and \\ P [n] = E [(x - \hat{x} [n | n - 1]) (^{x - \hat{x} [n | n - 1]) T}], \\ Z [n] = E [(x - \hat{x} [n | n]) (^{x - \hat{x} [n | n]) T}], \end{array}$

The steady-state Kalman filter uses a constant matrix P that minimizes the steady-state estimation error covariance and solves the associated discrete-time algebraic Riccati equation.

There are two variants of discrete-time Kalman filters:

The current estimator generates the state estimates $\hat{x} [n | n]$ using all measurement available, including y[n]. The filter updates $\hat{x} [n | n - 1]$ with y[n] and outputs:
$\begin{array}{l} \hat{x} [n | n] = \hat{x} [n | n - 1] + M [n] (y [n] - C [n] \hat{x} [n | n - 1] - D [n] u [n]), \\ \hat{y} [n | n] = C [n] \hat{x} [n | n] + D [n] u [n] . \end{array}$
The delayed estimator generates the state estimates $\hat{x} [n | n - 1]$ using measurements up to y[n –1]. The filter outputs $\hat{x} [n | n - 1]$ as defined previously, along with the optional output $\hat{y} [n | n - 1]$
$\hat{y} [n | n - 1] = C [n] \hat{x} [n | n - 1] + D [n] u [n]$

The current estimator has better estimation accuracy compared to the delayed estimator, which is important for slow sample times. However, it has higher computational cost, making it harder to implement inside control loops. More specifically, it has direct feedthrough. This leads to an algebraic loop if the Kalman filter is used in a feedback loop that does not contain any delays (the feedback loop itself also has direct feedthrough). The algebraic loop can impact the speed of simulation. You cannot generate code if your model contains algebraic loops.

The Kalman Filter block differs from the kalman command in the following ways:

When calling kalman(sys,...), sys includes the G and H matrices. Specifically, sys.B has [B G] and sys.D has [D H]. When you provide a LTI variable to the Kalman Filter block, it does not assume that the LTI variable provided contains G and H. They are optional and separate.
The kalman command outputs [yhat;xhat] by default. The block only outputs xhat by default.

Parameters

The following table summarizes the Kalman Filter block parameters, accessible via the Block Parameter dialog box.

Task	Parameters
Specify filter settings	Time domain Use the current measurement y[n] to improve xhat[n]
Specify the system model	Model source in Model Parameters tab
Specify initial state estimates	Source in Model Parameters tab
Specify noise characteristics	In Model Parameters tab: Use G and H matrices (default G=I and H=0) Q, Time-invariant Q R, Time-invariant R N, Time-invariant N
Specify additional inports	In Options tab: Add input port u Add input port Enable to control measurement updates External reset
Specify additional outports	In Options tab: Output estimated model output y Output state estimation error covariance Z

Time domain

Specify whether to estimate continuous-time or discrete-time states:

Discrete-Time (Default) — Block estimates discrete-time states
Continuous-Time — Block estimates continuous-time states
When the Kalman Filter block is in a model with synchronous state control (see the State Control (HDL Coder) block), you cannot select Continuous-time.

Use the current measurement y[n] to improve xhat[n]

Use the current estimator variant of the discrete-time Kalman filter. When not selected, the delayed estimator (variant) is used.

This option is available only when Time Domain is Discrete-Time.

Model source

Specify how the A, B, C, D matrices are provided to the block. Must be one of the following:

Dialog: LTI State-Space Variable — Use the values specified in the LTI state-space variable. You must also specify the variable name in Variable. The sample time of the model must match the setting in the Time domain option, i.e. the model must be discrete-time if the Time domain is discrete-time.
Dialog: Individual A, B, C, D matrices — Specify values in the following block parameters:
- A — Specify the A matrix. It must be real and square.
- B — Specify the B matrix. It must be real and have as many rows as the A matrix. This option is available only when Add input port u is selected in the Options tab.
- C — Specify the C matrix. It must be real and have as many columns as the A matrix.
- D — Specify the D matrix. It must be real. It must have as many rows as the C matrix and as many columns as the B matrix. This option is available only when Add input port u is selected in the Options tab.
External — Specify the A, B, C, D matrices as input signals to the Kalman Filter block. If you select this option, the block includes additional input ports A, B, C and D. You must also specify the following in the block parameters:
- Number of states — Number of states to be estimated, specified as a positive integer. The default value is 2.
- Number of inputs — Number of known inputs in the model, specified as a positive integer. The default value is 2. This option is only available when Add input port u is selected.
- Number of outputs — Number of measured outputs in the model, specified as a positive integer. The default value is 2.

Sample Time

Block sample time, specified as -1 or a positive scalar.

This option is available only when Time Domain is Discrete Time and Model Source is Dialog: Individual A, B, C, D matrices or External. The sample time is obtained from the LTI state-space variable if the Model Source is Dialog: LTI State-Space Variable.

The default value is -1, which implies that the block inherits its sample time based on the context of the block within the model. All block input ports must have the same sample time.

Source

Specify how to enter the initial state estimates and initial state estimation error covariance:

Dialog — Specify the values directly in the dialog box. You must also specify the following parameters:
- Initial states x[0] — Specify the initial state estimate as a real scalar or vector. If you specify a scalar, all initial state estimates are set to this scalar. If you specify a vector, the length of the vector must match with the number of states in the model.
- State estimation error covariance P[0] (only when time-varying Kalman filter is used) — Specify the initial state estimation error covariance P[0] for discrete-time Kalman filter or P(0) for continuous-time Kalman filter. Must be specified as one of the following:
  - Real nonnegative scalar. P is an Ns-by-Ns diagonal matrix with the scalar on the diagonals. Ns is the number of states in the model.
  - Vector of real nonnegative scalars. P is an Ns-by-Ns diagonal matrix with the elements of the vector on the diagonals of P.
  - Ns-by-Ns positive semi-definite matrix.
External — Inherit the values from input ports. The block includes an additional input port X0. A second additional input port P0 is added when time-varying Kalman filter is used. X0 and P0 must satisfy the same conditions described previously when you specify them in the dialog box.

Use the Kalman Gain K from the model variable

Specify whether to use the pre-identified Kalman Gain contained in the state-space plant model. This option is available only when:

Model Source is Dialog: LTI State-Space Variable and Variable is an identified state-space model (idss (System Identification Toolbox)) with a nonzero K matrix.
Time Invariant Q, Time Invariant R and Time Invariant N options are selected.

If the Use G and H matrices (default G=I and H=0) option is selected, Time Invariant G and Time Invariant H options must also be selected.

Use G and H matrices (default G=I and H=0)

Specify whether to use non-default values for the G and H matrices. If you select this option, you must specify:

G — Specify the G matrix. It must be a real matrix with as many rows as the A matrix. The default value is 1.
Time-invariant G — Specify if the G matrix is time invariant. If you unselect this option, the block includes an additional input port G.
H — Specify the H matrix. It must be a real matrix with as many rows as the C matrix and as many columns as the G matrix. The default value is 0.
Time-invariant H — Specify if the H matrix is time invariant. If you unselect this option, the block includes an additional input port G.
Number of process noise inputs — Specify the number of process noise inputs in the model. The default value is 1.
This option is available only when Time-invariant G and Time-invariant H are cleared. Otherwise, this information is inferred from the G or H matrix.

Q

Process noise covariance matrix, specified as one of the following:

Real nonnegative scalar. Q is an Nw-by-Nw diagonal matrix with the scalar on the diagonals. Nw is the number of process noise inputs in the model.
Vector of real nonnegative scalars. Q is an Nw-by-Nw diagonal matrix with the elements of the vector on the diagonals of Q.
Nw-by-Nw positive semi-definite matrix.

Time Invariant Q

Specify if the Q matrix is time invariant. If you unselect this option, the block includes an additional input port Q.

R

Measurement noise covariance matrix, specified as one of the following:

Real positive scalar. R is an Ny-by-Ny diagonal matrix with the scalar on the diagonals. Ny is the number of measured outputs in the model.
Vector of real positive scalars. R is an Ny-by-Ny diagonal matrix with the elements of the vector on the diagonals of R.
Ny-by-Ny positive-definite matrix.

Time Invariant R

Specify if the R matrix is time invariant. If you unselect this option, the block includes an additional input port R.

N

Process and measurement noise cross-covariance matrix. Specify it as a Nw-by-Ny matrix. The matrix [Q N; N^T R] must be positive definite.

Time Invariant N

Specify if the N matrix is time invariant. If you unselect this option, the block includes an additional input port N.

Add input port u

Select this option if your model contains known inputs u(t) or u[k]. The option is selected by default. Unselecting this option removes the input port u from the block and removes the B, D and Number of inputs parameters from the block dialog box.

Add input port Enable to control measurement updates

Select this option if you want to control the measurement updates. The block includes an additional inport Enable. The Enable input port takes a scalar signal. This option is cleared by default.

By default the block does measurement updates at each time step to improve the state and output estimates $\hat{x}$ and $\hat{y}$ based on measured outputs. The measurement update is skipped for the current sample time when the signal in the Enable port is 0. Concretely, the equation for state estimates become $\dot{\hat{x}} (t) = A (t) \hat{x} (t) + B (t) u (t)$ for continuous-time Kalman filter and $\hat{x} [n + 1 | n] = A [n] \hat{x} [n | n - 1] + B [n] u [n]$ for discrete-time.

External Reset

Option to reset estimated states and parameter covariance matrix using specified initial values.

Suppose you reset the block at a time step, t. If the block is enabled at t, the software uses the initial parameter values specified either in the block dialog or the input ports P0 and X0 to estimate the states. In other words, at t, the block performs a time update and if it is enabled, a measurement update after the reset. The block outputs these updated estimates.

Specify one of the following:

None (Default) — Estimated states $\hat{x}$ and state estimation error covariance matrix P values are not reset.
Rising — Triggers a reset when the control signal rises from a negative or zero value to a positive value. If the initial value is negative, rising to zero triggers a reset.
Falling — Triggers a reset when the control signal falls from a positive or a zero value to a negative value. If the initial value is positive, falling to zero triggers a reset.
Either — Triggers a reset when the control signal is either rising or falling.
Level — Triggers a reset in either of these cases:
- The control signal is nonzero at the current time step.
- The control signal changes from nonzero at the previous time step to zero at the current time step.
Level hold — Triggers reset when the control signal is nonzero at the current time step.

When you choose an option other than None, a Reset input port is added to the block to provide the reset control input signal.

Output estimated model output y

Add $\hat{y}$ output port to the block to output the estimated model outputs. The option is cleared by default.

Output state estimation error covariance P or Z

Add P output port or Z output port to the block. The Z matrix is provided only when Time Domain is Discrete Time and the Use the current measurement y[n] to improve xhat[n] is selected. Otherwise, the P matrix, as described in the Description section previously, is provided.

The option is cleared by default.

Ports

Port Name	Port Type (In/Out)	Description
u (Optional)	In	Known inputs, specified as a real scalar or vector.
y	In	Measured outputs, specified as a real scalar or vector.
xhat	Out	Estimated states, returned as a real scalar or vector.
yhat (Optional)	Out	Estimated outputs, returned as a real scalar or vector.
P or Z (Optional)	Out	State estimation error covariance, returned as a matrix.
A (Optional)	In	A matrix, specified as a real matrix.
B (Optional)	In	B matrix, specified as a real matrix.
C (Optional)	In	C matrix, specified as a real matrix.
D (Optional)	In	D matrix, specified as a real matrix.
G (Optional)	In	G matrix, specified as a real matrix.
H (Optional)	In	H matrix, specified as a real matrix.
Q (Optional)	In	Q matrix, specified as a real scalar, vector or matrix.
R (Optional)	In	R matrix, specified as a real scalar, vector or matrix.
N (Optional)	In	N matrix, specified as a real matrix.
P0 (Optional)	In	P matrix at initial time, specified as a real scalar, vector, or matrix.
X0 (Optional)	In	Initial state estimates, specified as a real scalar or vector.
Enable (Optional)	In	Control signal to enable measurement updates, specified as a real scalar.
Reset (Optional)	In	Control signal to reset state estimates, specified as a real scalar.

Supported Data Types

Double-precision floating point
Single-precision floating point (for discrete-time Kalman filter only)

Note

All input ports except Enable and Reset must have the same data type (single or double).
Enable and Reset ports support single, double, int8, uint8, int16, uint16, int32, uint32, and boolean data types.

Limitations

The plant and noise data must satisfy:
- (C,A) detectable
- $\bar{R} > 0$ and $\bar{Q} - \bar{N} {\bar{R}}^{- 1} {\bar{N}}^{T} \geq 0$
- $(A - \bar{N} {\bar{R}}^{- 1} C, \bar{Q} - \bar{N} {\bar{R}}^{- 1} {\bar{N}}^{T})$ has no uncontrollable mode on the imaginary axis (or unit circle in discrete time) with the notation
  $\begin{array}{l} \bar{Q} = G Q G^{T} \\ \bar{R} = R + H N + N^{T} H^{T} + H Q H^{T} \\ \bar{N} = G (Q H^{T} + N) \end{array}$
The continuous-time Kalman filter cannot be used in Function-Call Subsystems or Triggered Subsystems.

References

[1] Franklin, G.F., J.D. Powell, and M.L. Workman, Digital Control of Dynamic Systems, Second Edition, Addison-Wesley, 1990.

[2] Lewis, F., Optimal Estimation, John Wiley & Sons, Inc, 1986.

Documentation

Kalman Filter

Description

Parameters

Time domain

Use the current measurement y[n] to improve xhat[n]

Model source

Sample Time

Source

Use the Kalman Gain K from the model variable

Use G and H matrices (default G=I and H=0)

Q

Time Invariant Q

R

Time Invariant R

N

Time Invariant N

Add input port u

Add input port Enable to control measurement updates

External Reset

Output estimated model output y

Output state estimation error covariance P or Z

Ports

Supported Data Types

Limitations

References

Extended Capabilities

C/C++ Code Generation
Generate C and C++ code using Simulink® Coder™.

PLC Code Generation
Generate Structured Text code using Simulink® PLC Coder™.

See Also

Functions

Blocks

Topics

Control System Toolbox Documentation

Support

Documentation

Kalman Filter

Description

Parameters

Time domain

Use the current measurement y[n] to improve xhat[n]

Model source

Sample Time

Source

Use the Kalman Gain K from the model variable

Use G and H matrices (default G=I and H=0)

Q

Time Invariant Q

R

Time Invariant R

N

Time Invariant N

Add input port u

Add input port Enable to control measurement updates

External Reset

Output estimated model output y

Output state estimation error covariance P or Z

Ports

Supported Data Types

Limitations

References

Extended Capabilities

C/C++ Code Generation Generate C and C++ code using Simulink® Coder™.

PLC Code Generation Generate Structured Text code using Simulink® PLC Coder™.

See Also

Functions

Blocks

Topics

Control System Toolbox Documentation

Support

C/C++ Code Generation
Generate C and C++ code using Simulink® Coder™.

PLC Code Generation
Generate Structured Text code using Simulink® PLC Coder™.