Révision 470
Ajouté par Enzo LACHEZE il y a presque 3 ans
| branch/lacheze/sp4a3/sp4a3_kalman.c | ||
|---|---|---|
|
{
|
||
|
R[i][j]+=A[i][k]*B[k][j];
|
||
|
}
|
||
|
printf(" %lf", R[i][j]);
|
||
|
}
|
||
|
printf("\n");
|
||
|
}
|
||
|
|
||
|
}
|
||
| ... | ... | |
|
{0, 0, 1, 0},
|
||
|
{0, 0, 0, 1}};
|
||
|
double FT[4][4];
|
||
|
Transpose_Mat(4,4,F,FT);
|
||
|
Transpose_Mat(4,4,F,FT);
|
||
|
//création des matrices tampons, P(k+1|k)=P1, X(k+1|k)=X1, delta, P(k+1|k+1)=P2 et X(k+1|k+1)=X2
|
||
|
double obs[2][1]; //matrice (2,1) qui contient xobs et yobs
|
||
|
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 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); //calcul X2
|
||
|
//XK+1=X1 et PK+1=P1
|
||
|
//gain
|
||
|
Mul_Mat_Mat(4,4,P1,4,2,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); //calcul k
|
||
|
//MAJ
|
||
|
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 delta
|
||
|
Mul_Mat_Mat(4,2,K,2,1,delta,T9);
|
||
|
Add_Mat_Mat(4,1,X1,4,1,T9,X2); //calcul 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 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
creation code filtre kalman, test et validation du filtre
On a bien la courbe attendue