0
$\begingroup$

I'm trying to perform an hybrid kalman filter for a nonlinear system. The following code works but it is very slow due to the fact that it does symbolic evaluation.
What I wish is that for the filter equations first of all it does numeric substitution for the time dependent function and only then do all the actions specified by the algorithm step.
This is the code, only the last part is what really interests me.

(*SOME FUNCTIONS I'VE USED*)

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
]


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
]


(*SYSTEM PART*)
t0 = 0;
tfin = 4 Pi;
pstl = {xstl, ystl};
R0l[\[Theta]_] = {{Cos[\[Theta]], -Sin[\[Theta]]} , {Sin[\[Theta]], 
    Cos[\[Theta]]}};
pmk1 = {xmk1, ymk1};
pmk2 = {xmk2, ymk2};
numericalvalues = 
  Thread[{l1, l2, xstl, ystl, xmk1, ymk1, xmk2, ymk2} -> {8, 5, 
     3, -2, 32, 16, -15, 2}];


qalist = {x, y, \[Theta], xstls, ystls};
qa[t_] = (ToString[#] <> "[t]") & /@ qalist // ToExpression;
qad[t_] = D[qa[t], t];
qadd[t_] = D[qa[t], t];

v[t_] = 2;
\[Omega][t_] = 1;
input[t_] = {v[t], \[Omega][t]};

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


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



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
  ] (*Just create white noise continuous time sample*)


cov = {{0.008, 0}, {0, 
   0.008}}; (*(0.008//Sqrt) *3 \[Equal] 0.268 [m]*)
media = {0, 0};
noise = CreateWhiteNoise[media, cov, t0, tfin + 0.1, 0.04, t ];


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


eqdyn = qad[t] == dyn@qa[t];

qa0 = Flatten@{-2, 1, Pi/3, pstl} /. numericalvalues;
eqin = qa[0] == qa0;

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

{state, in, out} = 
  NDSolveValue[{eqdyn, eqin}, monitor, {t, t0, tfin}];

(*ParametricPlot[state[[1 ;; 2]], {t, t0, tfin}] -> circumference *)



(*FILTER PART*)
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[ToExpression[(ToString@Pmat[[#1, #2]] <> "[t]")] &, 
   Dimensions[Pmat]];
Pd[t_] = D[P[t], t];
P0 = IdentityMatrix[nstate]*{0.005, 0.005, 0.3 Degree, 0.005, 
    0.005 };
stima0 = RandomVariate[MultinormalDistribution[ qa0, P0]];
initstima = qahat[0] == stima0;
initcov = P[0] == P0 // SymRed;

predoutput = h@qahat[t] /. numericalvalues;
errstima = {xstls[t] - xstlshat[t], ystls[t] - ystlshat[t]};
A = D[dyn@qa[t], {qa[t]}] /. Thread[ qa[t] -> qahat[t]];
Ci = D[h@qa[t], {qa[t]}] /. Thread[qa[t] -> qahat[t]] /. 
   numericalvalues;
K = P[t].Ci\[Transpose].Inverse[cov];
errpred = hn@qa[t] - predoutput /. numericalvalues;

initEKF = {initstima, initcov};

monitorEKF = {qa[t], qahat[t], errstima};

(* INTERESTING PART - HYBRID KALMAN FILTER*)

dt = 0.1;

Sk = Ci.P[t].Ci\[Transpose] + cov;
Lk = P[t].Ci\[Transpose].Inverse[Sk];
updateP = (IdentityMatrix[nstate] - Lk.Ci).P[t].Transpose[
     IdentityMatrix[nstate] - Lk.Ci] + Lk.cov.Lk\[Transpose];
updateStima = qahat[t] + Lk.errpred;

eqCorr = WhenEvent[Mod[t, dt], 
   Evaluate@{qahat[t] -> updateStima, 
     P[t] -> updateP // SymRed, {"RestartIntegration"}}];

 eqPredStima = qahatd[t] == dyn@qahat[t];
eqPredCov =  Pd[t] == A.P[t] + P[t].A\[Transpose] // SymRed ;
eqEkfIbrid = {eqPredStima, eqPredCov, eqCorr};

{statoEkfIbrid, stimaEkfIbrid, errstimaEkfIbrid} = 
  NDSolveValue[{eqdyn, eqin, eqEkfIbrid, initEKF}, 
   monitorEKF, {t, t0, 2}, 
   Method -> {"EquationSimplification" -> "Residual"}];

Plot[errstimaEkfIbrid, {t, t0, 2}]

enter image description here

At the moment the code is terribly slow, I'm sure it can be improved by forcing in some way numerical evaluation.
An other question: What is the best best practice do formulate an algorithm as this one and more in general to make NDSolve give the best performances ? Do you think is better to explicit every time the arguments of each quantity so that we can use the _?NumericQ pattern test to force numerical evaluation ? I've also seen used for example NDSolve inside Block sometimes, does it help ?

$\endgroup$
8
  • $\begingroup$ It is not working code, some functions like eqns and altEKF not defined. $\endgroup$ Apr 7 '21 at 14:45
  • $\begingroup$ @AlexTrounev damn i'm gonna fix them. It should go now, thanks for having said $\endgroup$ Apr 7 '21 at 15:41
  • 1
    $\begingroup$ One step dt takes about 0.003 s on my laptop, but 20 steps take 93.4 s with your code. It means, that code nothing do during 93.4-20*0.003=93.34 s, and time can be reduced to 0.1 s. For this we can organize Module for one step computation and then use this module without WhenEvent. $\endgroup$ Apr 8 '21 at 10:22
  • $\begingroup$ @AlexTrounev I'm sorry for my less knowledge about the software, without WhenEvent how can I say to update the initial conditions every dt seconds ? $\endgroup$ Apr 9 '21 at 7:05
  • $\begingroup$ Actually Module has initial state and final state. While for WhenEvent we need to prepare some rule in general form and it takes 93.34 s with 20 steps, for Module we need to prepare numerical data for the next step, and it takes 0.1 s. $\endgroup$ Apr 9 '21 at 11:31

Your Answer

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

Browse other questions tagged or ask your own question.