To make long things short, here’s the complete Matlab code.
% State estimations x state vector (M x 1) A state transition matrix (M x M) P state covariance matrix (M x M) % Input / control data u input vector (N x 1) B input transition matrix (M x N) Q input noise covariance matrix (N x N) % Observations z observation vector (Z x 1) H state-to-observation matrix (Z x M) R observation noise covariance (Z x Z) % tuning lambda tuning parameter (scalar)
function [x, P] = kf_predict (x, A, P, lambda , u, B, Q) x = A*x + B*u; % a priori state prediction P = A*P*A' * 1/( lambda ^2) + B*Q*B'; % a priori covariance end
function [x, P] = kf_update (x, z, P, H, R)
y = z - H*x; % measurement residuals ("innovation")
S = H*P*H' + R; % residual (innovation) covariance
K = P*H' / S; % Kalman gain
x = x + K*y; % a posteriori state prediction
P = (eye(size(P)) - K*H)*P; % a posteriori covariance matrix
end
State-space system description
A discrete state-space system with the state vector \(\vec{x}_k\) at time index \(k\) as well as the control input \(\vec{u}_k\) to determine the current output \(\vec{y}_k\) of the system at time index \(k\) , as well as the next state \(\vec{x}_{k+1}\) at time index \(k+1\) can be written as follows:
\begin{align}
\vec{x}_{k+1} &= \underline{A}_{k} \cdot \vec{x}_k + \underline{B}_k \cdot \vec{u}_k \\
\vec{y}_k &= \underline{C}_{k} \cdot \vec{x}_k + \underline{D}_{k} \cdot \vec{u}_k
\end{align}
Here, \(\underline{A}\) is the state transition matrix that maps the current state vector \(\vec{x}_k\) into the future vector \(\vec{x}_{k+1}\) and \(\underline{B}\) is a matrix that transforms the control input \(\vec{u}\) onto the state vector \(\vec{x}\) . In the second equation, \(\underline{C}\) is a matrix that transforms the internal state \(\vec{x}_k\) into an output vector \(\vec{y}_k\) and \(\underline{D}\) is a matrix that describes an additive feedthrough effect of the control vector \(\vec{u}_k\) onto the output vector \(\vec{y}_k\) .
x_next = A*x + B*u; y = C*x + D*u; % then step forward in time k = k + 1; x = x_next;
Given that the linear system described above has a zero feedthrough term \(\underline{D} = \underline{0}\) (i.e. \(\underline{D}\) doesn’t exist), the regular Kalman filter’s equations are given as follows.
Kalman prediction equations (time update)
Let \(\tilde{\vec{x}}_{k|k}\) (read: the corrected \(\vec{x}\) at time \(k\) given information from time \(k\) ) be the current time-step state vector given any corrections using the last measurements — or the best guess we have of that state vector — and \(\tilde{\underline{P}}_{k|k}\) be its covariance (or its best guess we have).
Also let \(\underline{Q}_{i,k}\) be the matrix of the input covariances — i.e. the covariances in the input vector — and \(\underline{Q}_{p,k}\) the process noise covariance matrix.
If the control input vector is known
The next state \(\vec{x}_{k+1|k}\) (at time index $k+1$ given information of time \(k\) ) is estimated using the current (corrected) state \(\vec{x}_{k|k}\) and the current control input. The next state covariance \(\underline{P}_{k+1|k}\) is estimated using the current (corrected) covariance, the input covariance and the process noise, using:
\begin{align}
\hat{\vec{x}}_{k+1|k} &= \underline{A}_k \cdot \tilde{\vec{x}}_{k|k} + \underline{B}_k \cdot \vec{u}_{k} \\
\hat{\underline{P}}_{k+1|k} &= \underline{A}_k \cdot \tilde{\underline{P}}_{k|k} \cdot \underline{A}^T_k \cdot \frac{1}{\lambda^2_k} + \underline{B}_k \cdot \underline{Q}_{i,k} \cdot \underline{B}^T_k + \underline{Q}_{p,k}
\end{align}
% a priori x and P
estimated_next_x = A * corrected_current_x + B * current_u;
estimated_next_P = A * corrected_current_P * A' + 1/(lambda^2) + ...
B * Q_input * B' + Q_process;
If the control input vector is unknown
The next state \(\vec{x}_{k+1|k}\) (at time index \(k+1\) given information of time \(k\) ) is only estimated using the current (corrected) state \(\vec{x}_{k|k}\) while the next state covariance \(\underline{P}_{k+1|k}\) is estimated using only the current (corrected) covariance and the process noise, using:
\begin{align}
\hat{\vec{x}}_{k+1|k} &= \underline{A}_k \cdot \tilde{\vec{x}}_{k|k} \\
\hat{\underline{P}}_{k+1|k} &= \underline{A}_k \cdot \tilde{\underline{P}}_{k|k} \cdot \underline{A}^T_k \cdot \frac{1}{\lambda^2_k} + \underline{Q}_{p,k}
\end{align}
% a priori x and P estimated_next_x = A * corrected_current_x; estimated_next_P = A * corrected_current_P * A' + 1/(lambda^2) + Q_process;
In both cases, \( \lambda \) (introduced by Prof. Dr.-Ing. Sommer) with \( 0 \lambda \leq 1 \) is an empirical factor that forcibly increases the state covariance and thus the filter’s uncertainty in order to prevent convergence or to recover from convergence in situations where external state changes are known to have happened after the filter converged (when \( \underline{K}_{k+1} \cdot \tilde{\vec{y}}_{k+1} = 0\), see below). In the presence of a input and/or process noise covariance matrix \( \underline{Q}_{k}\) a value of \( \lambda \simeq 1\) (i.e. \( 0 \lll \lambda \leq 1\)) is recommended in order to prevent singularities or instabilities.
Kalman correction equations (measurement update)
Let \(\vec{z}\) be a vector of observations of the system and \(\underline{H}\) be a matrix that maps the state vector \(\vec{x}\) onto the observation vector \(\vec{z}\) under the influences of an error as described in the measurement noise covariance matrix \(\underline{R}\) .
Using the residual covariances \(\underline{S}\) (i.e. the error in the estimated covariances) and the innovation term \(\tilde{\vec{y}}\) (a measurement of the state estimation inaccuracy)
\begin{align}
\underline{S}_{k+1} &= \underline{H}_{k+1} \cdot \hat{\underline{P}}_{k+1|k} \cdot \underline{H}^T_{k+1} + \underline{R}_{k+1} \\
\tilde{\vec{y}}_{k+1} &= \vec{z}_{k+1} - \left( \underline{H}_{k+1} \cdot \hat{\vec{x}}_{k+1|k} \right)
\end{align}
S = H * estimated_next_P * H' + R; innovation = current_z - (H * estimated_next_x);
the eponymous Kalman Gain \(\underline{K}\) can be calculated as
\begin{align}
\underline{K}_{k+1} &= \hat{\underline{P}}_{k+1|k} \cdot \underline{H}^T_{k+1} \cdot \underline{S}^{-1}_{k+1}
\end{align}
K = estimated_next_P * H' / S; % note the inversion inv(S)
With that, the previously determined estimations \(\hat{\vec{x}}_{k+1|k}\) and \(\hat{\underline{P}}_{k+1|k}\) can be corrected using
\begin{align}
\tilde{\vec{x}}_{k+1|k+1} &= \hat{\vec{x}}_{k+1|k} + \underline{K}_{k+1} \cdot \tilde{\vec{y}}_{k+1} \\
\tilde{\underline{P}}_{k+1|k+1} &= \left( \underline{1} - \underline{K}_{k+1} \cdot \underline{H}_{k+1} \right) \cdot \hat{\underline{P}}_{k+1|k}
\end{align}
With \(\underline{1}\) being the unit matrix of the same dimensions as \(\underline{P}\) (i.e. the number of states × the number of states).
% a posteriori x and P corrected_next_x = estimated_next_x + K * innovation; corrected_next_P = (eye(size(P)) - K * H) * estimated_next_P;
An alternative (and more complex) way to calculate the a posteriori covariance matrix in presence of a non-optimal Kalman gain is known as the Joseph form and is given as
\begin{align}
\underline{J}_k &= \left( \underline{1} - \underline{K}_{k+1} \cdot \underline{H}_{k+1} \right) \\
\tilde{\underline{P}}_{k+1|k+1} &= \underline{J} \cdot \hat{\underline{P}}_{k+1|k} \cdot \underline{J}^T + \underline{K}_{k+1} \cdot \underline{R}_{k+1} \cdot \underline{K}^T_{k+1}
\end{align}
% a posteriori P J = (eye(size(K)) - K * H); corrected_next_P = J*estimated_next_P*J' + K*R*K;
However, assuming that the Kalman gain is actually optimal is often a reasonably good approach. After that, we move forward in time.
k = k + 1; corrected_current_x = corrected_next_x; corrected_current_P = corrected_next_P;
As can be seen in the Kalman gain equation, for any reasonably complex system with more than three states, the inversion of the residual covariance matrix \(\underline{S}\) gets exponentially computationally intensive. This matrix is symmetric positive-definite though, so the Cholesky decomposition can be used to efficiently invert the matrix. Code for that can be found e.g. in the Efficient Java Matrix Library or as a C++ port in my own Kalman filter library kalman-clib (see cholesky_decompose_lower() for decomposition into a lower triangular matrix and matrix_invert_lower() for inversion of a lower-triangularized matrix).
