Projet

Général

Profil

« Précédent | Suivant » 

Révision 276

Ajouté par gasacco il y a presque 4 ans

Codage filtre Kalman

Voir les différences:

branch/sacco/sp4a3/sp4a3_kalman.c
double RST44[4][4]={{-11,-11,-7,-11},{-15,-17,-16,-32},{-69,-37,-5,3},{-49,-6,-9,-45}};
double RMT44T44[4][4]={{816,412,192,304},{1155,583,379,687},{1146,668,466,758},{486,308,222,338}};
double Y1[2][2] = {{5,2},{3,8}};
printf("Execution des tests unitaires.\n");
......
Transpose_Mat(4,4,T44a,R44); if (!Equal_Mat_Mat(RTT44,R44)) error("Erreur calcul Transposition 4x4");
Inverse_Mat_22(2,2,T22a,R22); if (!Equal_Mat_Mat(RInvT22,R22)) error("Erreur calcul Inversion 2x2");
//Add_Mat_Mat(2,2,T22b, 4,2, R22, R42); if (Equal_Mat_Mat(RAT22, R42) error("Erreur calcul Y1");
Add_Mat_Mat(2,2,T22a,2,2,T22b,R22); if (!Equal_Mat_Mat(RAT22,R22)) error("Erreur calcul Addition 2x2");
Add_Mat_Mat(4,4,T44a,4,4,T44b,R44); if (!Equal_Mat_Mat(RAT44,R44)) error("Erreur calcul Addition 4x4");
Add_Mat_Mat(4,1,T41a,4,1,T41b,R41); if (!Equal_Mat_Mat(RAT41,R41)) error("Erreur calcul Addition 4x1");
......
double FT[4][4];
Transpose_Mat(4,4,F,FT);
double tampon_1[4][4];
double tampon_2[2][4];
double tampon_44[4][4];
double tampon_44bis[4][4];
double tampon_24[2][4];
double tampon_22[2][2];
double tampon_22bis[2][2];
double tampon_22ter[2][2];
double tampon_42[4][2];
double tampon_41[4][1];
double tampon_21[2][1];
double DELTA[2][1];
double Xcopie[4][1];
double Pcopie[4][4];
while(fscanf(fichier, "%lf %lf %lf", &t, &x, &y)>0){
printf("-------------%04d--------------\n",cpt);
......
// Kalman
// X = F*X
Mul_Mat_Mat(4,4,F, 4,1, X, X);
Mul_Mat_Mat(4,4,F, 4,1, X, tampon_41);
Plot_Mat(X," X(k+1|k) = ");
//P = F*P*F'+Q;
Mul_Mat_Mat(4,4,F,4,4,P,tampon_1);
Mul_Mat_Mat(4,4,tampon_1,4,4, FT, tampon_1);
Add_Mat_Mat(4,4,tampon_1,4,4,Q, P);
Mul_Mat_Mat(4,4,F,4,4,P,tampon_44);
Mul_Mat_Mat(4,4,tampon_44,4,4, FT, tampon_44bis);
Add_Mat_Mat(4,4,tampon_44bis,4,4,Q, P);
Plot_Mat(P,"P(k+1|k) = F.P(k|k).FT + Q = ");
// K = P*H' / ( H*P*H' + R);
Mul_Mat_Mat(4,4,P,4,2, HT, K);
Mul_Mat_Mat(2,4,H,4,4,P, tampon_2);
Mul_Mat_Mat(2,4,tampon_2, 4,2,HT, tampon_2);
Add_Mat_Mat(2,2,tampon_2,2,2,R, tampon_2);
Inverse_Mat_22(2,2,tampon_2, tampon_2);
Mul_Mat_Mat(4,2,K, 2,2, tampon_2, K);
Mul_Mat_Mat(4,4,P,4,2, HT, tampon_42);
Mul_Mat_Mat(2,4,H,4,4,P, tampon_24);
Mul_Mat_Mat(2,4,tampon_24, 4,2,HT, tampon_22);
Add_Mat_Mat(2,2,tampon_22,2,2,R, tampon_22bis);
Inverse_Mat_22(2,2,tampon_22bis, tampon_22ter);
Mul_Mat_Mat(4,2,tampon_42, 2,2, tampon_22ter, K);
Plot_Mat(K,"K = ");
//X = X + K*([xb(i);yb(i)]-H*X);
//Plot_Mat(Delta,"DELTA = Obs - H.X(k+1|k)");
Plot_Mat(X," X(k+1|k+1) = X(k+1|k) + K.Delta = ");
//X = X + K*([xb(i);yb(i)]-H*X);
double Obs[2][1];
Obs[0][0] = x;
Obs[1][0] = y;
Mul_Mat_Mat(2,4, H, 4,1, X, tampon_21);
Sub_Mat_Mat(2,1, Obs,2,1, tampon_21, DELTA);
Plot_Mat(Obs," Obs = [2][1] ");
Mul_Mat_Mat(4,2,K,2,1, DELTA,tampon_41);
Add_Mat_Mat(4,1, X, 4,1, tampon_41, Xcopie );
// P = P - K*H*P;
Plot_Mat(P," P(k+1|k+1) = P(k+1|k) - K.H.P(k+1|k) = ");
//Plot_Mat(Delta,"DELTA = Obs - H.X(k+1|k)");
/// La matrice X doit contenir la position filtrée ///
Plot_Mat(Xcopie," 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, tampon_44);
Mul_Mat_Mat(4,4,tampon_44, 4,4, P, tampon_44bis);
Sub_Mat_Mat(4,4,P, 4,4, tampon_44bis, Pcopie);
Plot_Mat(Pcopie," P(k+1|k+1) = P(k+1|k) - K.H.P(k+1|k) = ");
/// La matrice X doit contenir la position filtrée ///
X[0][0] = Xcopie[0][0];
X[1][0] = Xcopie[1][0];
X[2][0] = Xcopie[2][0];
X[3][0] = Xcopie[3][0];
P[0][0] = Pcopie[0][0];
P[1][0] = Pcopie[1][0];
P[2][0] = Pcopie[2][0];
P[3][0] = Pcopie[3][0];
}
t = cpt * dt;
dx = (x - oldx)/dt;

Formats disponibles : Unified diff