function [x0f,x1p,Pf,P]=kalman(x0,u0,ym,A,B,C,D,P,Q,R)
% Simple Kalman filter of autoregressive model implementation
%
% Input parameters:
%   x0 .... state x[n]
%   u0 .... input u[n]
%   ym .... measured y[n]
%   A ..... state development matrix A
%   B ..... input scaling matrix B
%   C ..... output matrix E(y[n]) = C*x[n]
%           may be time-variable
%   D ..... intercept matrix D (typically empty)
%   P ..... a posteriori estimate covariance of x[n]
%   Q ..... process noise covariance
%   R ..... observation noise covariance
%
% Output parameters:
%   x0f ... filtered state x[n]
%   x1p ... predicted state x[n+1] based on x0f
%   Pf .... a posteriori estimate of x[n] covariance (filterted)
%   P ..... a priori estimate of x[n+1] covariance
%

% -----------
% Update step
% -----------
% Output prediction
yp = C*x0 + D*u0;
% Innovation or measurement residual
ey = ym - yp;
% Innovation covariance
S = C*P*C'+R;
% Optimal Kalman gain
% We try to avoid computing the inverse of S
K = (P*C')/S;
% Original: K = P*C'*inv(S);
% Updated (a posteriori) state estimate
x0f = x0 + K*ey;
% Updated (a posteriori) estimate covariance
Pf = P - K*C*P;

% ---------------
% Prediction step 
% ---------------
% New predicted state
x1p = A*x0f + B*u0;
% The predicted (a priori) estimate covariance is the filtered covariance
% scaled by the state development matrix + process noise
P = A*Pf*A' + Q;

