5
$\begingroup$

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!

$\endgroup$
4
$\begingroup$

(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}

enter image description here

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]}
|improve this answer|||||
$\endgroup$
  • $\begingroup$ 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! $\endgroup$ – morbo May 30 '19 at 19:37
  • $\begingroup$ @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. $\endgroup$ – Suba Thomas May 31 '19 at 13:53
  • $\begingroup$ 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? $\endgroup$ – morbo Jun 9 '19 at 9:48
  • $\begingroup$ 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. $\endgroup$ – Suba Thomas Jun 10 '19 at 13:41

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service, privacy policy and cookie policy

Not the answer you're looking for? Browse other questions tagged or ask your own question.