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
