1
$\begingroup$

I just simulate a system and use an Extended Kalman Filter to estimate the state. Everything works, but I don't understand why I need to execute the Ndsolve command two times before it can run without error.
Here is the code (maybe it can seem to be intricate but there is no need to understand it, however, is just a simulation of a dynamical system and a filter):

t0 = 0;
tfin = 200;
SeedRandom[1234];
pstl = {xstl, ystl};
R0l[\[Theta]_] = {{Cos[\[Theta]], -Sin[\[Theta]]} , {Sin[\[Theta]], 
    Cos[\[Theta]]}};
pmk1 = {xmk1, ymk1};
pmk2 = {xmk2, ymk2};
numericalvalues = 
  Thread[{xstl, ystl, xmk1, ymk1, xmk2, ymk2} -> { 3, -2, 32, 16, -15,
      2}];


(*Configuration variables*)

qalist = {x, y, \[Theta], xstls, ystls};
qa[t_] = (ToString[#] <> "[t]") & /@ qalist // ToExpression;
qad[t_] = D[qa[t], t];
qadd[t_] = D[qad[t], t];
input[t_] = {v[t], \[Omega][t]};
output[t_] = {y1[t], y2[t], yn1[t], yn2[t]};


(*Desired Trajectory*)

dgl[t_] = {-15 Cos[t] + 4, 15 Sin[t] + 4};
Rgl[t_] = {{Cos[-t], -Sin[-t]}, {Sin[-t], Cos[-t]}};
pl[t_] = {Sin[7 t], 0};
tr = dgl[t/2] + Rgl[t/2].pl[t/2];
vdes = D[tr, t];
ades = D[vdes, t];

(*System Dynamics*)
g1 = {Cos[#], Sin[#], 0, 0, 0} & @(#[[3]]) &;
g2 = {0, 0, 1, 0, 0};
dyn = (g1@#*v[t] + g2*\[Omega][t]) &;

(*Output functions*)
pstf = ({#1, #2} + R0l[#3 ].{#4, #5}) &;
h1 = (Sqrt[#.#] &[pmk1 - pstf @@ #]) &;
h2 = (Sqrt[#.#] &[pmk2 - pstf @@ # ]) &;


(*Just create white noise continuous time sample*)

CreateWhiteNoise[\[Mu]_, s_, t0_, tfin_, dt_, p_] := 
 Module[{distr, n = Length[\[Mu]], listt, nc, listrn},
    listt = Range[t0, tfin, dt];
    nc = Length[listt];
    distr = MultinormalDistribution[\[Mu], s];
    listrn = RandomVariate[distr, nc];
    (Interpolation[Thread[Join[{listt, listrn[[All, #]]}]]][p]) & /@ 
   Range@n
  ] 

(*Noise*)
cov = {{0.008, 0}, {0, 0.008}}; 
media = {0, 0};
noise = CreateWhiteNoise[media, cov, t0, tfin + 0.1, 0.2, t ];

(*Output functins with additive noise*)
h = {h1@#, h2@#} & ;
hn = (h@# + noise) &;
output[t_] = Join[h@qa[t], hn@qa[t]] /. numericalvalues;


(*Controller*)
pos = qa[t][[1 ;; 2]];
vel = qad[t][[1 ;; 2]];
acc = qadd[t][[1 ;; 2]];
Kv = 3*IdentityMatrix[2];
Kp = 3*IdentityMatrix[2];
ev = vdes - vel;
ep = tr - pos;
\[Nu] = ades + Kv.ev + Kp.ep;

monitor = {qa[t], input[t], output[t]};

v[t_] = Sqrt[x'[t]^2 + y'[t]^2]; 
\[Omega][t_] = (y''[t] * x'[t] - x''[t]*y'[t]) / (x'[t]^2 + y'[t]^2);

{y1[t_], y2[t_], yn1[t_], yn[t_]} = 
  Join[h@qa[t], hn@qa[t]] /. numericalvalues;

eqcontrollo = acc == \[Nu] ;
\[Theta]d = D[ ArcTan[x'[t], y'[t]], t];
eqtheta = \[Theta]'[t] == \[Theta]d;
eqsens = {xstls'[t], ystls'[t]} == {0, 0};
(*Dynamic Equations*)

eqns = {eqcontrollo, qad[t][[3 ;; 5]] == {\[Theta]d, 0, 0}} /. 
   numericalvalues;

(*Initial conditions*)
qa0 = {-2, 1, \[Pi]/3, 3, -2};
eqin = {x[0], y[0], \[Theta][0], xstls[0], ystls[0], 
    Derivative[1][x][0], Derivative[1][y][0]} == {-2, 1, \[Pi]/3, 
    3, -2, 1/20, Sqrt[3]/20};

{stato, ingressi, uscite} = 
  NDSolveValue[{eqns, eqin}, monitor, {t, t0, tfin}];

ParametricPlot[stato[[1 ;; 2]], {t, t0, tfin}, ImageSize -> Tiny];

(*FILTER*)

(*Auxiliary functions*)

ListForm[mat_] := 
 DeleteCases[(Thread@# & /@ Thread@mat // Flatten ), True]
SymRed[mat_] := Module[{i, j, temp},
    temp = Thread@# & /@ Thread@mat;
     Normal@
    SparseArray[{{i_, j_} /; i >= j :> 
       temp[[i, j]], {i_, j_} /; i < j :> True}, Dimensions@temp] // 
   ListForm
  ]

qlisthat = (ToString@# <> "hat" // ToExpression) & /@ qalist ;
qahat[t_] = (ToString@# <> "[t]" // ToExpression) & /@ qlisthat;
qahatd[t_] = D[qahat[t], t];

nstate = Length@qlisthat;

Pmat = Normal@
   SparseArray[{{i_, j_} /; i >= j :> 
      ToExpression["p" <> ToString@i <> ToString@j],
                        {i_, j_} /; i < j :> 
      ToExpression["p" <> ToString@j <> ToString@i ]}, 
    nstate*{1, 1}];
P[t_] = Array[(ToString@Pmat[[#1, #2]] <> "[t]") &, 
    Dimensions[Pmat]] // ToExpression;
Pd[t_] = D[P[t], t];

P0 = IdentityMatrix[nstate]*{0.005, 0.005, 0.3 Degree, 0.005, 
     0.005 } // Chop;
stima0 = RandomVariate[MultinormalDistribution[ qa0, P0]];
initstima = qahat[0] == stima0;
initcov = P[0] == P0 ;

Rinv = Inverse[cov] // Chop;

predoutput = h@qahat[t] /. numericalvalues;

errstima = qa[t] - qahat[t];

A = D[dyn@qahat[t], {qahat[t]}];
Ci = D[h@qahat[t], {qahat[t]}] /. numericalvalues;
K = P[t].Ci\[Transpose].Rinv;
errpred = hn@qa[t] - predoutput /. numericalvalues;

initEKF = {initstima, initcov};

eqStima = qahatd[t] == dyn@qahat[t] + K.errpred;
eqP = SymRed[
   Pd[t] == 
    A.P[t] + P[t].A\[Transpose] - P[t].Ci\[Transpose].Rinv.Ci.P[t]];
eqEKF = {eqStima, eqP} /. numericalvalues;

last part of code:


errstimaEKF = 
  NDSolveValue[{eqns, eqin, eqEKF, initEKF}, errstima, {t, t0, tfin}, 
   WorkingPrecision -> 12, MaxStepSize -> 0.001, MaxSteps -> 10^6];

Plot[errstimaEKF, {t, t0, tfin}, 
 PlotLegends -> ("e" <> ToString@# & /@ qalist), PlotRange -> All]

This should be the result:

enter image description here

It happens only if I specify MaxStepSize option in Ndsolve or if tfin is high.

QUESTION 2 The code is terribly slow,someone knows if is there a way to manipulate the Ndsolve input (ODE) to speed up the code ?
Let's say for example using := instead of = or using functions instead of expressions, maybe with the pattern test _?NumericQ in the arguments. In the code above is there a way to manipulate the ODE in a way that they contain functions evaluated only numerically instead of expressions? Maybe using something the numeric pattern test only on the time variable t

Improve this question
$\endgroup$

1 Answer 1

Reset to default
3
$\begingroup$

To speed up computation we should simplifier all equations and initial conditions first by Flatten all list in list in list and so on. Also we can increase MaxStepSize -> 0.01 since it is not affected result, finally we have

Clear["Global`*"]

t0 = 0;
tfin = 200;
SeedRandom[1234];
pstl = {xstl, ystl};
R0l[\[Theta]_] = {{Cos[\[Theta]], -Sin[\[Theta]]}, {Sin[\[Theta]], 
    Cos[\[Theta]]}};
pmk1 = {xmk1, ymk1};
pmk2 = {xmk2, ymk2};
numericalvalues = 
  Thread[{xstl, ystl, xmk1, ymk1, xmk2, ymk2} -> {3, -2, 32, 16, -15, 
     2}];


(*Configuration variables*)

qalist = {x, y, \[Theta], xstls, ystls};
qa[t_] = (ToString[#] <> "[t]") & /@ qalist // ToExpression;
qad[t_] = D[qa[t], t];
qadd[t_] = D[qad[t], t];
input[t_] = {v[t], \[Omega][t]};
output[t_] = {y1[t], y2[t], yn1[t], yn2[t]};


(*Desired Trajectory*)

dgl[t_] = {-15 Cos[t] + 4, 15 Sin[t] + 4};
Rgl[t_] = {{Cos[-t], -Sin[-t]}, {Sin[-t], Cos[-t]}};
pl[t_] = {Sin[7 t], 0};
tr = dgl[t/2] + Rgl[t/2] . pl[t/2];
vdes = D[tr, t];
ades = D[vdes, t];

(*System Dynamics*)
g1 = {Cos[#], Sin[#], 0, 0, 0} &@(#[[3]]) &;
g2 = {0, 0, 1, 0, 0};
dyn = (g1@#*v[t] + g2*\[Omega][t]) &;

(*Output functions*)
pstf = ({#1, #2} + R0l[#3] . {#4, #5}) &;
h1 = (Sqrt[# . #] &[pmk1 - pstf @@ #]) &;
h2 = (Sqrt[# . #] &[pmk2 - pstf @@ #]) &;


(*Just create white noise continuous time sample*)

CreateWhiteNoise[\[Mu]_, s_, t0_, tfin_, dt_, p_] := 
 Module[{distr, n = Length[\[Mu]], listt, nc, listrn}, 
  listt = Range[t0, tfin, dt];
  nc = Length[listt];
  distr = MultinormalDistribution[\[Mu], s];
  listrn = RandomVariate[distr, nc];
  (Interpolation[Thread[Join[{listt, listrn[[All, #]]}]]][p]) & /@ 
   Range@n]

(*Noise*)
cov = {{0.008, 0}, {0, 0.008}};
media = {0, 0};
noise = CreateWhiteNoise[media, cov, t0, tfin + 0.1, 0.2, t];

(*Output functins with additive noise*)
h = {h1@#, h2@#} &;
hn = (h@# + noise) &;
output[t_] = Join[h@qa[t], hn@qa[t]] /. numericalvalues;


(*Controller*)
pos = qa[t][[1 ;; 2]];
vel = qad[t][[1 ;; 2]];
acc = qadd[t][[1 ;; 2]];
Kv = 3*IdentityMatrix[2];
Kp = 3*IdentityMatrix[2];
ev = vdes - vel;
ep = tr - pos;
\[Nu] = ades + Kv . ev + Kp . ep;

monitor = {qa[t], input[t], output[t]};

v[t_] = Sqrt[x'[t]^2 + y'[t]^2];
\[Omega][t_] = (y''[t]*x'[t] - x''[t]*y'[t])/(x'[t]^2 + y'[t]^2);

{y1[t_], y2[t_], yn1[t_], yn[t_]} = 
  Join[h@qa[t], hn@qa[t]] /. numericalvalues;

eqcontrollo = acc - \[Nu];

\[Theta]d = D[ArcTan[x'[t], y'[t]], t];
eqtheta = \[Theta]'[t] == \[Theta]d;
eqsens = {xstls'[t], ystls'[t]} == {0, 0};
(*Dynamic Equations*)


eqns = ({eqcontrollo, qad[t][[3 ;; 5]] - {\[Theta]d, 0, 0}}) /. 
    numericalvalues // Flatten;

(*Initial conditions*)

qa0 = {-2, 1, \[Pi]/3, 3, -2};
eqin = {x[0], y[0], \[Theta][0], xstls[0], ystls[0], 
    Derivative[1][x][0], Derivative[1][y][0]} - {-2, 1, \[Pi]/3, 
    3, -2, 1/20, Sqrt[3]/20};

{stato, ingressi, uscite} = 
  NDSolveValue[{Table[eqns[[i]] == 0, {i, Length[eqns]}], 
    Table[eqin[[i]] == 0, {i, Length[eqin]}]}, monitor, {t, t0, tfin}];

ParametricPlot[stato[[1 ;; 2]], {t, t0, tfin}, ImageSize -> Tiny]

(*FILTER*)

(*Auxiliary functions*)

ListForm[mat_] := 
 DeleteCases[(Thread@# & /@ Thread@mat // Flatten), True]
SymRed[mat_] := Module[{i, j, temp}, temp = Thread@# & /@ Thread@mat;
  Normal@SparseArray[{{i_, j_} /; i >= j :> 
       temp[[i, j]], {i_, j_} /; i < j :> True}, Dimensions@temp] // 
   ListForm]

qlisthat = (ToString@# <> "hat" // ToExpression) & /@ qalist;
qahat[t_] = (ToString@# <> "[t]" // ToExpression) & /@ qlisthat;
qahatd[t_] = D[qahat[t], t];

nstate = Length@qlisthat;

Pmat = Normal@
   SparseArray[{{i_, j_} /; i >= j :> 
      ToExpression["p" <> ToString@i <> ToString@j], {i_, j_} /; 
       i < j :> ToExpression["p" <> ToString@j <> ToString@i]}, 
    nstate*{1, 1}];
P[t_] = Array[(ToString@Pmat[[#1, #2]] <> "[t]") &, 
    Dimensions[Pmat]] // ToExpression;
Pd[t_] = D[P[t], t];

P0 = IdentityMatrix[nstate]*{0.005, 0.005, 0.3 Degree, 0.005, 0.005} //
    Chop;
stima0 = RandomVariate[MultinormalDistribution[qa0, P0]];
initstima = qahat[0] - stima0;
initcov = P[0] - P0;

Rinv = Inverse[cov] // Chop;

predoutput = h@qahat[t] /. numericalvalues;

errstima = qa[t] - qahat[t];

A = D[dyn@qahat[t], {qahat[t]}];
Ci = D[h@qahat[t], {qahat[t]}] /. numericalvalues;
K = P[t] . Ci\[Transpose] . Rinv;
errpred = hn@qa[t] - predoutput /. numericalvalues;

initEKF = Join[initstima, initcov] // Flatten;


eqStima = qahatd[t] - dyn@qahat[t] - K . errpred;
eqP = SymRed[-Pd[t] + A . P[t] + P[t] . A\[Transpose] - 
    P[t] . Ci\[Transpose] . Rinv . Ci . P[t]];
eqEKF = {eqStima, eqP} /. numericalvalues // Flatten;

errstimaEKF = 
   NDSolveValue[{Table[eqns[[i]] == 0, {i, Length[eqns]}], 
     Table[eqin[[i]] == 0, {i, Length[eqin]}], 
     Table[eqEKF[[i]] == 0, {i, Length[eqEKF]}], 
     Table[initEKF[[i]] == 0, {i, Length[initEKF]}]}, 
    errstima, {t, t0, tfin}, MaxStepSize -> 0.01, MaxSteps -> 10^6, 
    Method -> {"EquationSimplification" -> 
       "Residual"}]; // AbsoluteTiming

Now it takes 10 s on my laptop. Visualization in two scales

{Plot[errstimaEKF, {t, t0, tfin}, 
  PlotLegends -> ("e" <> ToString@# & /@ qalist), PlotRange -> All], 
 Plot[errstimaEKF, {t, t0, tfin}, 
  PlotLegends -> ("e" <> ToString@# & /@ qalist)]}

Figure 1

Improve this answer
$\endgroup$
3
  • $\begingroup$ thank you very much I didn't know NDSolve prefers equations in list form ! $\endgroup$
    – PeaceEverybody
    Commented Apr 27, 2021 at 13:54
  • $\begingroup$ Do you think in general is the mine a good way to afford the problem ? working with expressions instead of functions, or is it pretty equal for Mathematica ? $\endgroup$
    – PeaceEverybody
    Commented Apr 27, 2021 at 13:55
  • 1
    $\begingroup$ @PeaceEverybody Yes, it is, for NDSolve we need to prepare equations in a simplified form since in your case there is system of algebraic-differential equations. Also it could be better to compile some functions to speed up computation. $\endgroup$
    – Alex Trounev
    Commented Apr 27, 2021 at 14:58

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.