Révision 569
Ajouté par Louis JACQUET il y a presque 3 ans
| sp4a3_kalman.c | ||
|---|---|---|
|
double FT[4][4];
|
||
|
Transpose_Mat(4,4,F,FT);
|
||
|
|
||
|
double P1[4][4]; // declaration matrice pour calcul
|
||
|
double P1[4][4]; // declaration matrice pour les calculs
|
||
|
double P2[4][4];
|
||
|
|
||
|
double R1[4][4];
|
||
| ... | ... | |
|
double R9[4][4];
|
||
|
double R10[4][4];
|
||
|
|
||
|
double DELT[2][1];
|
||
|
double DELTA[2][1];
|
||
|
|
||
|
|
||
|
|
||
| ... | ... | |
|
|
||
|
debug=1; ///Mettre à 1 pour afficher les matrices.
|
||
|
///Ajouter votre code ci-dessous///
|
||
|
// Kalman
|
||
|
// Kalman
|
||
|
double Obs[2][1] = {{xobs},{yobs}};
|
||
|
|
||
|
// X = F*X
|
||
|
Mul_Mat_Mat(4,4,F,4,1,X,X1);
|
||
| ... | ... | |
|
|
||
|
Plot_Mat(K,"K = ");
|
||
|
|
||
|
//Delt
|
||
|
//Delta
|
||
|
Mul_Mat_Mat(2,4,H,4,1,X1,R7);
|
||
|
Sub_Mat_Mat(2,1,Obs,2,1,R7,DELT);
|
||
|
Sub_Mat_Mat(2,1,Obs,2,1,R7,DELTA);
|
||
|
|
||
|
Plot_Mat(DELT,"DELT = Obs - H.X(k+1|k)");
|
||
|
Plot_Mat(DELTA,"DELTA = Obs - H.X(k+1|k)");
|
||
|
|
||
|
//X = X + K*([xobs(i);yobs(i)]-H*X) = X + K*delt;
|
||
|
Mul_Mat_Mat(4,2,K,2,1,DELT,R8);
|
||
|
//X = X + K*([xobs(i);yobs(i)]-H*X) = X + K*delta;
|
||
|
Mul_Mat_Mat(4,2,K,2,1,DELTA,R8);
|
||
|
Add_Mat_Mat(4,1,X1,2,1,R8,X2);
|
||
|
|
||
|
Plot_Mat(X," X(k+1|k+1) = X(k+1|k) + K.DELT = ");
|
||
|
Plot_Mat(X," X(k+1|k+1) = X(k+1|k) + K.Delta = ");
|
||
|
|
||
|
// P = P - K*H*P;
|
||
|
Mul_Mat_Mat(4,2,K,2,4,H,R9);
|
||
Formats disponibles : Unified diff
Version finale filtre de Kalman, TP pas si compliqué mais assez casse tête et trouve sa difficulté dans l'incapacité a trouver ses erreurs malgré un build correct.