Révision 632
Ajouté par jcguifodjo il y a environ 4 ans
| sp4a3_kalman.c | ||
|---|---|---|
|
Plot_Mat(X," X(k+1|k) = ");
|
||
|
|
||
|
//P = F*P*F'+Q;
|
||
|
double A0[4][4],A1[4][4],A2[4][2],A3[2][4],A4[2][4],B[2][2],D[2][2],C[2][2],E[2][2];
|
||
|
double delta[2][1],Obs[2][1];
|
||
|
double A0[4][4],A1[4][4],A2[4][2],A3[4][4],A4[4][4],B[2][2],D[2][2],C[2][2],E[2][1];
|
||
|
double delta[2][1],Obs[2][1],C1[4][1];
|
||
|
Mul_Mat_Mat(4,4,F,4,4,P,A0);
|
||
|
Mul_Mat_Mat(4,4,A0,4,4,FT,A1);
|
||
|
Add_Mat_Mat(4,4,A1,4,4,Q,P);
|
||
| ... | ... | |
|
Mul_Mat_Mat(2,4,H,4,2,A2,B);
|
||
|
Add_Mat_Mat(2,2,B,2,2,R,C);
|
||
|
Inverse_Mat_22(2,2,C,D) ;
|
||
|
Mul_Mat_Mat(2,2,A2,2,2,D,K);
|
||
|
Mul_Mat_Mat(4,2,A2,2,2,D,K);
|
||
|
Plot_Mat(K,"K = ");
|
||
|
|
||
|
//X = X + K*([xb(i);yb(i)]-H*X);
|
||
|
|
||
|
//Plot_Mat(Delta,"DELTA = Obs - H.X(k+1|k)");
|
||
|
|
||
|
|
||
|
Mul_Mat_Mat(2,4,H,4,1,X,E);
|
||
|
Sub_Mat_Mat(2,1,Obs,2,1,E,delta);
|
||
|
Mul_Mat_Mat(4,2,K,2,1,delta,C1);
|
||
|
Add_Mat_Mat(4,1,X,4,1,C1,X);
|
||
|
Plot_Mat(X," X(k+1|k+1) = X(k+1|k) + K.Delta = ");
|
||
|
|
||
|
// P = P - K*H*P;
|
||
|
Mul_Mat_Mat(2,2,K,2,4,H,A3);
|
||
|
Mul_Mat_Mat(2,4,A3,4,4,P,A4);
|
||
|
Sub_Mat_Mat(4,4,P,2,4,A4,P);
|
||
|
Mul_Mat_Mat(4,2,K,2,4,H,A3);
|
||
|
Mul_Mat_Mat(4,4,A3,4,4,P,A4);
|
||
|
Sub_Mat_Mat(4,4,P,4,4,A4,P);
|
||
|
Plot_Mat(P," P(k+1|k+1) = P(k+1|k) - K.H.P(k+1|k) = ");
|
||
|
|
||
|
/// La matrice X doit contenir la position filtrée ///
|
||
Formats disponibles : Unified diff
Implémentation de la fonction calcul vitesse
Et implémentation de la matrice observation du filtre de Kalman