How to implement a Kalman Filter

Below I use low level code to reproduce the example of a Kalman Filter on pages 16-20 found here. First I create the data.

meas={0.32,0.42,0.13,0.48,0.82,0.7,0.93,0.84,0.78,0.76,1.35,1.2,1.07,1.3,1.8,
1.3,1.77,2.0,2.1,1.77};
len=Length@meas;
measurement@0=truth@0=0;
Do[truth[i]=0.1*i;
measurement[i]=Part[meas,i],
{i,1,len,1}
];


Next I filter the data.

r=0.1;
q=0.00001;
H={{1,0},{0,0}};
A={{1,1},{0,1}};
X[0]={{0.0},{0}};
P[0]={{1,0},{0,1}};
Q=q*10^-9*{{1/3,0.5},{0.5,1}};
R=10^-7{{r,0},{0,r}};
KalmanGain[0]=P[0].Transpose[H].Inverse[H .P[0].Transpose[H]+R];
Do[
(X[k]=A.X[k-1];
P[k]=A.P[k-1].Transpose[A]+Q;
KalmanGain[k]=P[k].Transpose[H].Inverse[H .P[k].Transpose[H]+R];
X[k]=X[k]+KalmanGain[k].(measurement[k]-H.X[k]);
P[k]=(IdentityMatrix[2]-KalmanGain[k].H).P[k]),
{k,len}
];


Then I plot the results and get the same result as the plot at the above web-page.

measurementPlot=ListLinePlot[{#,measurement@#}&/@Range[0,len],
Mesh->All,PlotStyle->{Thickness@0.001,Black},
MeshStyle->{AbsolutePointSize@7,Black}
];
filteredPlot=ListLinePlot[{#,Part[X@#,1,1]}&/@Range[0,len],
Mesh->All,PlotStyle->{Thickness@0.001,Magenta},
MeshStyle->{AbsolutePointSize@7,Magenta}
];
truePlot=ListLinePlot[{#,truth@#}&/@Range[0,len],Mesh->All,
PlotStyle->{Thickness@0.001,Yellow},
MeshStyle->{AbsolutePointSize@7,Yellow}
];
Show[measurementPlot,filteredPlot,truePlot,GridLinesStyle->Black,
AxesStyle->Black,Background->GrayLevel@0.8,
GridLines->{{},{0.5,1.0,1.5,2.0}},PlotRange->{0,2.25},
ImageSize->{550,400},AspectRatio->0.8]


How could I used KalmanEstimator or similar built-in functions to get the same result?

• Have you looked at this documentation article? – m_goldberg Nov 8 '15 at 12:15
• @m_goldberg, I read that part of the documentation before. However, it's still very hard to see how that example can be modified to the application above. – Ted Ersek Nov 8 '15 at 18:49