Révision 510
Ajouté par Orlani RIVERA il y a presque 3 ans
| branch/RIVERA_Orlani/sp4a3/sp4a3_kalman.c | ||
|---|---|---|
|
int i,j;
|
||
|
na=nb;
|
||
|
ma=mb;
|
||
|
for (i=0;i<na;i++
|
||
|
for (i=0;i<na;i++)
|
||
|
{
|
||
|
for (j=0;j<ma;j++)
|
||
|
{
|
||
| ... | ... | |
|
{0, 0, 0, 1}};
|
||
|
double FT[4][4];
|
||
|
Transpose_Mat(4,4,F,FT);
|
||
|
/* création de matrices tampons, P(k+1|k)=P1, X(k+1|k), delta, P(k+1|k+1)=P2 et X(k+1|k+1)=X2 */
|
||
|
double obs[2][1];
|
||
|
double X1[4][1];
|
||
|
double T1[4][4];
|
||
|
double T2[4][4];
|
||
|
double P1[4][4];
|
||
|
double T3[4][2];
|
||
|
double T4[2][4];
|
||
|
double T5[2][2];
|
||
|
double T6[2][2];
|
||
|
double T7[2][2];
|
||
|
double delta[2][1];
|
||
|
double T8[2][1];
|
||
|
double T9[4][1];
|
||
|
double X2[4][1];
|
||
|
double T10[4][4];
|
||
|
double T11[4][4];
|
||
|
double P2[4][4];
|
||
|
|
||
|
while(fscanf(fichier, "%lf %lf %lf", &t, &xobs, &yobs)>0){
|
||
|
printf("-------------%04d--------------\n",cpt);
|
||
| ... | ... | |
|
|
||
|
debug=0; ///Mettre à 1 pour afficher les matrices.
|
||
|
///Ajouter votre code ci-dessous///
|
||
|
// Kalman
|
||
|
// Kalman
|
||
|
/* Prediction */
|
||
|
Mul_Mat_Mat(4,4,F,4,1,X,X1); /* Calcul de X1 */
|
||
|
Mul_Mat_Mat(4,4,F,4,4,P,T1);
|
||
|
Mul_Mat_Mat(4,4,T1,4,4,FT,T2);
|
||
|
Add_Mat_Mat(4,4,T2,4,4,Q,P1);
|
||
|
|
||
|
/* gain */
|
||
|
Mul_Mat_Mat(4,4,P1,4,4,HT,T3);
|
||
|
Mul_Mat_Mat(2,4,H,4,4,P1,T4);
|
||
|
Mul_Mat_Mat(2,4,T4,4,2,HT,T5);
|
||
|
Add_Mat_Mat(2,2,T5,2,2,R,T6);
|
||
|
Inverse_Mat_22(2,2,T6,T7);
|
||
|
Mul_Mat_Mat(4,2,T3,2,2,T7,K);
|
||
|
|
||
|
Mul_Mat_Mat(2,4,H,4,1,X1,T8);
|
||
|
obs[0][0]=xobs;
|
||
|
obs[1][0]=yobs;
|
||
|
Sub_Mat_Mat(2,1,obs,2,1,T8,delta); /* calcul de delta */
|
||
|
Mul_Mat_Mat(4,2,K,2,1,delta,T9);
|
||
|
Add_Mat_Mat(4,1,X1,4,1,T9,X2); /* calcul de X2 */
|
||
|
Mul_Mat_Mat(4,2,K,2,4,H,T10);
|
||
|
Mul_Mat_Mat(4,4,T10,4,4,P1,T11);
|
||
|
Sub_Mat_Mat(4,4,P1,4,4,T11,P2); /* Calcul de P2 */
|
||
|
|
||
|
//Affichage
|
||
|
|
||
|
// X = F*X
|
||
|
Plot_Mat(X," X(k+1|k) = ");
|
||
| ... | ... | |
|
// P = P - K*H*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 ///
|
||
|
/// La matrice X doit contenir la position filtrée ///
|
||
|
/* La matrice X prend les valeurs de la matrice X2 et la matrice P prend les valeurs de la matrice P2 */
|
||
|
int a,b;
|
||
|
for (a=0;a<4;a++)
|
||
|
{
|
||
|
X[a][0]=X2[a][0];
|
||
|
}
|
||
|
for (a=0;a<4;a++)
|
||
|
{
|
||
|
for (b=0;b<4;b++)
|
||
|
{
|
||
|
P[a][b]=P2[a][b];
|
||
|
}
|
||
|
}
|
||
|
}
|
||
|
t = cpt * dt;
|
||
|
dx = (xobs - oldx)/dt;
|
||
Formats disponibles : Unified diff
question 6 du TP3.