Révision 471
Ajouté par Hatim EL MAADI il y a presque 3 ans
| sp4a3_kalman.c | ||
|---|---|---|
|
{
|
||
|
t -= t0;xobs -= x0;yobs -= y0;
|
||
|
|
||
|
debug=0; ///Mettre à 1 pour afficher les matrices.
|
||
|
debug=1; ///Mettre à 1 pour afficher les matrices.
|
||
|
///Ajouter votre code ci-dessous///
|
||
|
// Kalman
|
||
|
// Kalman
|
||
|
double T44[4][4]={{0,0,0,0},{0,0,0,0},{0,0,0,0},{0,0,0,0}};
|
||
|
double T24[2][4]={{0,0,0,0},{0,0,0,0}};
|
||
|
double T42[4][2]={{0,0},{0,0},{0,0},{0,0}};
|
||
|
double T22[2][2]={{0,0},{0,0}};
|
||
|
double T21[2][1]={{0},{0}};
|
||
|
double T41[4][1]={{0},{0},{0},{0}};
|
||
|
double Z1[4][1];
|
||
|
double Z2[4][4];
|
||
|
double Z3[2][2];
|
||
|
double Z4[4][1];
|
||
|
double Z5[4][4];
|
||
|
/*Prédiction*/
|
||
|
Plot_Mat(X," X(k|k) = ");
|
||
|
|
||
|
// X = F*X
|
||
|
Plot_Mat(X," X(k+1|k) = ");
|
||
|
// X = F*X
|
||
|
Mul_Mat_Mat(4,4,F,4,1,X,Z1);
|
||
|
Add_Mat_Mat(4,1,Z1,4,1,T41,X);
|
||
|
Plot_Mat(X," X(k+1|k) = ");
|
||
|
Plot_Mat(P,"P(k|k) = ");
|
||
|
|
||
|
//P = F*P*F'+Q;
|
||
|
Mul_Mat_Mat(4,4,F,4,4,P,T44);
|
||
|
Mul_Mat_Mat(4,4,T44,4,4,FT,Z2);
|
||
|
Add_Mat_Mat(4,4,Z2,4,4,Q,P);
|
||
|
Plot_Mat(P,"P(k+1|k) = F.P(k|k).FT + Q = ");
|
||
|
|
||
|
/*Gain*/
|
||
|
// K = P*H' / ( H*P*H' + R);
|
||
|
Plot_Mat(K,"K = ");
|
||
|
|
||
|
//X = X + K*([xobs(i);yobs(i)]-H*X);
|
||
|
Mul_Mat_Mat(4,4,P,4,2,HT,T42);
|
||
|
Plot_Mat(T42,"P(k+1|k).HT = ");
|
||
|
Mul_Mat_Mat(2,4,H,4,2,T42,T22);
|
||
|
Add_Mat_Mat(2,2,T22,2,2,R,Z3);
|
||
|
Plot_Mat(Z3,"H.P(k+1|k).HT + R = ");
|
||
|
Inverse_Mat_22(2,2,Z3,T22);
|
||
|
Plot_Mat(T22,"INV(H.P(k+1|k).HT + R) = ");
|
||
|
Mul_Mat_Mat(4,2,HT,2,2,T22,T42);
|
||
|
Mul_Mat_Mat(4,4,P,4,2,T42,K);
|
||
|
Plot_Mat(K,"K = ");
|
||
|
/*Mise_à_jour*/
|
||
|
double vecteur [2][1]={{xobs},{yobs}};
|
||
|
Plot_Mat(vecteur,"obs = ");
|
||
|
//DELTA = Obs - H.X(k+1|k)
|
||
|
double Delta [2][1];
|
||
|
Mul_Mat_Mat(2,4,H,4,1,X,T21);
|
||
|
Sub_Mat_Mat(2,1,vecteur,2,1,T21,Delta);
|
||
|
Plot_Mat(Delta,"DELTA = Obs - H.X(k+1|k) = ");
|
||
|
//X = X + K*([xobs(i);yobs(i)]-H*X);
|
||
|
Mul_Mat_Mat(4,2,K,2,1,Delta,T41);
|
||
|
Add_Mat_Mat(4,1,X,4,1,T41,Z4);
|
||
|
init(4,1,T41); //Initialiser T41, T41=0
|
||
|
Add_Mat_Mat(4,1,T41,4,1,Z4,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 = ");
|
||
|
|
||
|
// P = P - K*H*P;
|
||
|
// P = P - K*H*P;
|
||
|
Mul_Mat_Mat(2,4,H,4,4,P,T24);
|
||
|
Mul_Mat_Mat(4,2,K,2,4,T24,T44);
|
||
|
Sub_Mat_Mat(4,4,P,4,4,T44,Z5);
|
||
|
init(4,4,T44);//mettre T44 à 0
|
||
|
Add_Mat_Mat(4,4,Z5,4,4,T44,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 ///
|
||
|
}
|
||
|
t = cpt * dt;
|
||
|
/// La matrice X doit contenir la position filtrée ///
|
||
|
|
||
|
}
|
||
|
|
||
|
|
||
|
t = cpt * dt;
|
||
|
dx = (xobs - oldx)/dt;
|
||
|
dy = (yobs - oldy)/dt;
|
||
|
fprintf(Fout,"%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\n",t,xobs,yobs,sqrt(dx*dx+dy*dy)*dt,X[0][0],X[1][0],X[2][0],X[3][0],sqrt(X[2][0]*X[2][0]+X[3][0]*X[3][0])*dt);
|
||
| ... | ... | |
|
}
|
||
|
fclose(Fout);
|
||
|
fclose(fichier);
|
||
|
|
||
|
|
||
|
|
||
|
system ("gnuplot -p -e \"plot 'output.dat' u 5:6 w l, '' u 2:3 w l\";");
|
||
|
system ("gnuplot -p -e \"plot 'output.dat' u 1:9 w l, '' u 1:4 w l\";");
|
||
|
system ("gnuplot -p -e \"plot 'output.dat' u 9 w l , 'vitesse_reelle.dat' u 2 w l\";");
|
||
|
return 0;
|
||
|
}
|
||
|
|
||
|
}
|
||
Formats disponibles : Unified diff
Programme Final du TP, Filtre de Kalman marche bien.