2D tracking example

Initializations

sw0=10.0;
Q=DiagonalMatrix[{sw0,sw0,.2*sw0,.2*sw0}];
ndistw=MultinormalDistribution[{0,0,0,0},Q];
w:=RandomReal[ndistw];

sv0=250.0;
R0={{sv0,0.0},{0.0,sv0}};
ndistv=MultinormalDistribution[{0,0},R0];
v:=RandomReal[ndistv];

Generate synthetic data: Model true trajectory plus noise

Initialize and generate

iter=100;
y0={.0,0.0,.1,.1};

A ={{1,0,1,0},{0,1,0,1},{0,0,1,0},{0,0,0,1}};
H={{1,0,0,0},{0,1,0,0}};

y[k_]:= A.k+w
ys=NestList[y,y0,iter-1];
x=H.#&/@ys+RandomReal[ndistv,iter];

True path

g2=ListPlot[H.#&/@ys,Joined->True]

Noisy measurements

g1=ListPlot[x]

Estimate path from data

Initialize arrays

yh=Table[{0,0,0,0},{iter}];
K=Table[{{0,0},{0,0},{0,0},{0,0}},{iter}];
P=Table[DiagonalMatrix[{0,0,0,0}],{iter}];
Pm=P;
yhp=yh;

Initial estimates

R=100R0; (* Change measurement noise assumption. If R is small, then the data is trusted--the dots get joined. If the data is believed to be noisy, the state estimate is smoothed *)

Graph here gets dynamically updated when the Kalman fiter section below is evaluated

The path moves from right to left.

Dynamic[gxys=ListPlot[{x,H.#&/@ys,H.#&/@yhp},Joined->{False,True,True}]]

Kalman filter

For[t=1,t<iter,t++,
(*Time update--predict the state and error covariance *)
yh[[t+1]]=A.yhp[[t]];
Pm[[t+1]]=A.P[[t]].Transpose[A]+Q;

(*Measurement update--correction*)
K[[t+1]]=Pm[[t+1]].(H^\[Transpose]).Inverse[H.Pm[[t+1]].(H^\[Transpose]) + R];

P[[t+1]]=(IdentityMatrix[4]-K[[t+1]].H).Pm[[t+1]];

yhp[[t+1]]=yh[[t+1]]+K[[t+1]].(x[[t+1]]-H.yh[[t+1]]);
(*gxys=ListPlot[{x,H.#&/@ys,H.#&/@yhp},Joined->{False,True,True}];*)
Pause[.05];
];