Révision 468
Ajouté par Stephen LOPEZ il y a environ 3 ans
| branch/LOPEZ_Stephen/tp_sp4_2022_LOPEZ_Stephen/sp4a3/sp4a3_kalman.c | ||
|---|---|---|
|
{0, 0, 0, 1}};
|
||
|
double FT[4][4];
|
||
|
Transpose_Mat(4,4,F,FT);
|
||
|
|
||
|
|
||
|
|
||
|
double X1[4][1];
|
||
|
double A0[4][4];
|
||
|
double A1[4][4];
|
||
|
double P1[4][4];
|
||
|
double B0[4][2];
|
||
|
double B1[2][4];
|
||
|
double B2[2][2];
|
||
|
double B3[2][2];
|
||
|
double B4[2][2];
|
||
|
double DELTA[2][1];
|
||
|
double obs[2][1];
|
||
|
double C0[2][1];
|
||
|
double X2[4][1];
|
||
|
double D0[2][1];
|
||
|
double E0[4][4];
|
||
|
double E1[4][4];
|
||
|
double P2[4][4];
|
||
|
|
||
|
while(fscanf(fichier, "%lf %lf %lf", &t, &xobs, &yobs)>0){
|
||
|
printf("-------------%04d--------------\n",cpt);
|
||
| ... | ... | |
|
|
||
|
|
||
|
|
||
|
|
||
|
|
||
|
// X = F*X
|
||
|
// X = F*X
|
||
|
Mul_Mat_Mat(4,4,F,4,1,X,X1);
|
||
|
Plot_Mat(X," X(k+1|k) = ");
|
||
|
|
||
|
//P = F*P*F'+Q;
|
||
|
//P = F*P*F'+Q;
|
||
|
Mul_Mat_Mat(4,4,F,4,4,P,A0);//F*P
|
||
|
Mul_Mat_Mat(4,4,A0,4,4,FT,A1);//A0*F'
|
||
|
Add_Mat_Mat(4,4,A1,4,4,Q,P1);//A1+Q
|
||
|
Plot_Mat(P,"P(k+1|k) = F.P(k|k).FT + Q = ");
|
||
|
|
||
|
// K = P*H' / ( H*P*H' + R);
|
||
|
Plot_Mat(K,"K = ");
|
||
|
// K = P*H' / ( H*P*H' + R);
|
||
|
Mul_Mat_Mat(4,4,P1,4,2,HT,B0);//P*H'
|
||
|
Mul_Mat_Mat(2,4,H,4,4,P1,B1);//H*P
|
||
|
Mul_Mat_Mat(2,4,B1,4,2,HT,B2);//B1*HT
|
||
|
Add_Mat_Mat(2,2,B2,2,2,R,B3);//B2+R
|
||
|
Inverse_Mat_22(2,2,B3,B4);//B3T
|
||
|
Mul_Mat_Mat(4,2,B0,2,2,B4,K);//B0*B4
|
||
|
Plot_Mat(K,"K = ");
|
||
|
|
||
|
//X = X + K*([xobs(i);yobs(i)]-H*X);
|
||
|
//Plot_Mat(Delta,"DELTA = Obs - H.X(k+1|k)");
|
||
|
//Plot_Mat(Delta,"DELTA = Obs - H.X(k+1|k)");
|
||
|
Mul_Mat_Mat(2,4,H,4,1,X1,D0);//H*X1
|
||
|
obs[0][0]=xobs;
|
||
|
obs[1][0]=yobs;
|
||
|
Sub_Mat_Mat(2,1,obs,2,1,D0,DELTA);
|
||
|
|
||
|
//X = X + K*([xobs(i);yobs(i)]-H*X);
|
||
|
Mul_Mat_Mat(4,2,K,2,1,DELTA,C0);
|
||
|
Add_Mat_Mat(4,1,X1,4,1,C0,X2);
|
||
|
|
||
|
Plot_Mat(X," X(k+1|k+1) = X(k+1|k) + K.Delta = ");
|
||
|
|
||
|
|
||
|
// P = P - K*H*P;
|
||
|
Plot_Mat(P," P(k+1|k+1) = P(k+1|k) - K.H.P(k+1|k) = ");
|
||
|
|
||
|
Mul_Mat_Mat(4,2,K,2,4,H,E0);
|
||
|
Mul_Mat_Mat(4,4,E0,4,4,P1,E1);
|
||
|
Sub_Mat_Mat(4,4,P1,4,4,E1,P2);
|
||
|
Plot_Mat(P," P(k+1|k+1) = P(k+1|k) - K.H.P(k+1|k) = ");
|
||
|
|
||
|
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];
|
||
|
}
|
||
|
}
|
||
|
|
||
|
/// La matrice X doit contenir la position filtrée ///
|
||
|
}
|
||
|
t = cpt * dt;
|
||
Formats disponibles : Unified diff
TP SPn°3 est terminé, le filtre de Kalman fonctionne.
ENFINN !!!