### Kalman Filter5. MATLAB (complete)

% KF: System X+ = AX + V, Y = CX + W (by X+ we mean X(n+1))

% \Sigma_V = Q^2, \Sigma_W = R^2

% we construct a gaussian noise V = normrnd(0, Q), W = normrnd(0, R)

% where Z is iid N(0, 1)

% The filter is Xh+ = (A - KCA)Xh + KY (Here Xh is the estimate)

% K+ = SC'[CSC' + R^2)

% S = AHA' + Q^2 (Here H = \Sigma = est. error cov.)

% H+ = (1 - KC)S

% CONSTANTS

A = [1, 1; 0, 1];

C = [1, 0];

Q = [1; 0];

R = 0.5;

SV = Q*Q';

SW = R*R';

N = 20; %time steps

M = length(A);

%FILTER

S = A*H*A' + SV; %Gain calculation

K = S*C'*(C*S*C'+SW)^(-1);

H = (1 - K*C)*S;

%Estimate update

Xh(:,n+1) = (A - K*C*A)*Xh(:,n) + K*Y;

end

%PLOT

P = [X(2,:); Xh(2,:)]';

plot(P)

%SYSTEM

X = zeros(M, N);

Xh = X;

H = zeros(M, M);

K = H;

X(:,1) = [0; 3]; %initial state

for n = 1:N-1% These are the system equations

V = normrnd(0,Q)

W = normrnd(0,R;);

X(:,n+1) = A*X(:,n) + V;

Y = C*X(:,n+1) + W;