# Extended Kalman Filter by hand

While trying to learn a bit about control theory, observers and most importantly, to use Microcontroller Kit in version 12 of MMA, I've stumbled upon the KalmanEstimator.

After reading a bit (and some more bits) about it, and playing around I stumbled upon the extended Kalman observer/filter in the NonlinearStateSpaceModel documentation.

Reproduced here:

a = {0, 0, 0, 0};
b = {{Cos[\[Theta] + \[Phi]], 0}, {Sin[\[Theta] + \[Phi]],
0}, {Sin[\[Theta]], 0}, {0, 1}};
xx = {x, y, \[Phi], \[Theta]};
robot = AffineStateSpaceModel[{a, b, {x, y, \[Theta]}}, {x, y, \[Phi], \[Theta]}, {v, w}]

R = 0.1 IdentityMatrix[3];
P = Array[Subscript[p, ##] &, {4, 4}];
B = D[{x, y, \[Theta]}, {xx}];
K = P.B\[Transpose].Inverse[R] // Chop;

rhs1 = a + b.{v, w} + P.B\[Transpose].Inverse[R].({Subscript[x, m], Subscript[y, m], Subscript[\[Theta], m]} - {x, y, \[Theta]});
A = D[a + b.{v, w}, {xx}];
rhs2 = Flatten[A.P + P.A\[Transpose] - P.B[Transpose].Inverse[R].B.P];
rhs = Join[rhs1, rhs2];
join1 = Join[xx, Flatten@MapThread[Rule, {Array[Subscript[p, ##] &, {4, 4}], 10 IdentityMatrix[4]}, 2]];

ekf = NonlinearStateSpaceModel[{rhs, {x, y, \[Theta]}}, join1, {v, w, Subscript[x, m], Subscript[y, m], Subscript[\[Theta], m]}]


This is all fine. It's a nice relatively clear example when using a differentiable function described in matrix b I decided I'd try it on a system I've been using for testing and filtering signal noise that is also nonlinear.

dgl = \[Phi]''[t] + g/l Sin[ \[Phi][t]] == -0.07 ArcTan[100 Derivative[1][\[Phi]][t]] - 0.4 Derivative[1][\[Phi]][t] + u[t];
nsys = NonlinearStateSpaceModel[{dgl}, {{\[Phi][t], 0}, {\[Phi]'[t], 0}}, {u[t]}, {\[Phi][t]}, t] /. {g -> 9.81, l -> 2}


At this point, I mostly attempted to modify step by step the example to fit my situation (the way I understood it...which as you guessed it, I didn't).

xx = {\[Phi][t], \[Phi]'[t]};
a = {0, 0};
b = {\[Phi]''[t] + g/l Sin[ \[Phi][t]] == -0.07 ArcTan[100 Derivative[1][\[Phi]][t]] - 0.4 Derivative[1][\[Phi]][t] + u[t], 0}
R = 0.1 IdentityMatrix[3];
P = Array[Subscript[p, ##] &, {4, 4}];
B = D[{\[Phi][t], \[Phi]'[t]}, {xx}];
K = P.B\[Transpose].Inverse[R] // Chop;
rhs1 = a + b.{u[t]} + P.B\[Transpose].Inverse[R].({Subscript[\[Phi], m], Subscript[\[Phi]', m]} - {\[Phi], \[Phi]'});


Here mostly just changing the dimensions of the matrices and replacing variables names to match my own (and I'm guessing B is also wrong but anyways...).

However at this point I'm now at a loss on how to continue. My system is described in a single line ODE, while the example uses a matrix of trig functions and does matrix math from here on to derive rhs1 and rhs2.

How can I continue from here onwards to build the rhs1 and rhs2 parts of this extended filter example using my ODE? Is this just a matter of writing my ODE in the appropriate dimension matrix B and continuing on?

If there is a better method to design such a filter using ODEs I'd love an example but not required at all. Regardless thanks for the help in advance!

(Although you have not understood it correctly, you have shown a good effort in understanding it. IMO, it would be easier for you if you first had a basic understanding of the EKF, before trying to code it.)

The following fixes all aspects where you have gone wrong so far.

The AffineStateSpaceModel:

asys = AffineStateSpaceModel[{dgl}, {{\[Phi][t], 0}, {\[Phi]'[t], 0}},
{u[t]}, {\[Phi][t]}, t] /. {g -> 9.81, l -> 2}


Get the model parameters:

{{a, b, c, d}, xx, uu, yy, t} = Normal[asys];
xx = First /@ xx;
{a, b, xx} = {a, b, xx} /. Thread[xx -> {\[Phi], \[Omega]}];


Begin assembling the filter:

R = 0.1 IdentityMatrix[1];
P = Array[Subscript[p, ##] &, {2, 2}];
B = D[{\[Phi]}, {xx}];
K = P.B\[Transpose].Inverse[R] // Chop;
rhs1 = a + b.{u} + P.B\[Transpose].Inverse[R].{Subscript[\[Phi], m]-\[Phi]}

• Thank you @Suba Thomas...the filter now works like expected with your corrections...Though, a quick question, I noticed the filter only seems to give me an accurate result when the input to the filter is "Join[input,noisysig[t]]" namely the input I used on the asys, joined with my generated noisy measurement. Do I understand that correctly that the filter when deployed to the MCU would also require the knowledge of inputs on top of the noisy signal? Or am I missing something? thanks again! – morbo May 30 '19 at 19:37
• @morbo. The Kalman filter needs to know any deterministic input signal. A noisy input is not needed, but its covariance must be known and will be part of rhs2. – Suba Thomas May 31 '19 at 13:53
• Ahh does that not make the ekf useless to use on a microcontroller, if it's unknown when inputs come into the system?...such as disturbance resistance to taps in an inverse pendulum? – morbo Jun 9 '19 at 9:48
• Not really, because all though not accounted for in the model, it could make itself known to the filter through the measurements and sometimes the control input if there is a controller. If it is an unstable system, then any filter by itself, without a controller, makes no sense. If that disturbance is not modeled and also unobservable, then, of course, the filter will be useless. – Suba Thomas Jun 10 '19 at 13:41