Révision 455
Ajouté par rademagalh il y a presque 4 ans
| branch/DEMAGALHAES/sp4a3/sp4a3_kalman.c | ||
|---|---|---|
|
}
|
||
|
|
||
|
int main(int argc,char **argv){
|
||
|
|
||
|
tests_unitaires();
|
||
|
|
||
|
FILE* fichier = fopen("pos_t_x_y.dat","r");
|
||
|
if (fichier == NULL)
|
||
| ... | ... | |
|
|
||
|
debug=1; ///Mettre à 1 pour afficher les matrices.
|
||
|
///Ajouter votre code ci-dessous///
|
||
|
// Kalmans
|
||
|
// Kalmans
|
||
|
|
||
|
Plot_Mat(X,"X(k|k) = ");
|
||
|
// X = F*X
|
||
|
Mul_Mat_Mat(4,4,F,4,1,X,Xk);
|
||
|
Plot_Mat(Xk," X(k+1|k) = ");
|
||
|
|
||
|
Mul_Mat_Mat(4,4,F,4,1,X,X);
|
||
|
Plot_Mat(X," X(k+1|k) = ");
|
||
|
|
||
|
Plot_Mat(P,"P(k|k) = ");
|
||
|
//P = F*P*F'+Q;
|
||
|
|
||
|
double A[4][4];
|
||
|
double B[4][4];
|
||
|
// A = F*P
|
||
| ... | ... | |
|
Transpose_Mat(4,4,F,FT);
|
||
|
Mul_Mat_Mat(4,4,A,4,4,FT,B);
|
||
|
// P = B+Q
|
||
|
Add_Mat_Mat(4,4,B,4,4,Q,P);
|
||
|
double Pk[4][4];
|
||
|
Add_Mat_Mat(4,4,B,4,4,Q,Pk);
|
||
|
Plot_Mat(Pk,"P(k+1|k) = F.P(k|k).FT + Q = ");
|
||
|
|
||
|
|
||
|
Plot_Mat(P,"P(k+1|k) = F.P(k|k).FT + Q = ");
|
||
|
|
||
|
// K = P*H' / ( H*P*H' + R);
|
||
|
// K = C /E
|
||
|
// K = C * /E
|
||
|
double C[4][2];
|
||
|
double HT[4][2];
|
||
|
Transpose_Mat(2,4,H,HT);
|
||
|
Mul_Mat_Mat(4,4,P,4,2,HT,C);
|
||
|
Mul_Mat_Mat(4,4,Pk,4,2,HT,C);
|
||
|
Plot_Mat(C,"P(k+1|k).HT = ");
|
||
|
|
||
|
double D[2][4];
|
||
|
Mul_Mat_Mat(2,4,H,4,4,P,D);
|
||
|
Mul_Mat_Mat(2,4,H,4,4,Pk,D);
|
||
|
double E[2][2];
|
||
|
Mul_Mat_Mat(2,4,D,4,2,HT,E);
|
||
|
Add_Mat_Mat(2,2,E,2,2,R,E);
|
||
|
Plot_Mat(E,"H.P(k+1|k).HT + R = ");
|
||
|
|
||
|
double E_inv[2][2];
|
||
|
Inverse_Mat_Mat(2,2,E,E_inv);
|
||
|
Plot_Mat(E_inv,"INV(H.P(k+1|k).HT + R) = ");
|
||
|
|
||
|
Mul_Mat_Mat(4,2,C,2,2,E,K);
|
||
|
Mul_Mat_Mat(2,2,E_inv,4,2,C,K);
|
||
|
Plot_Mat(K,"K = ");
|
||
|
|
||
|
|
||
|
double obs[2][1]={{x},{y}};
|
||
|
Plot_Mat(obs,"Obs = ");
|
||
|
// DELTA = obs - H*X
|
||
|
double DELTA[2][1];
|
||
|
double obs[2][1]={{x},{y}};
|
||
|
double Z[2][1];
|
||
|
Mul_Mat_Mat(2,4,H,4,1,X,Z);
|
||
|
Mul_Mat_Mat(2,4,H,4,1,Xk,Z);
|
||
|
Sub_Mat_Mat(2,1,obs,2,1,Z,DELTA);
|
||
|
Plot_Mat(DELTA,"DELTA = Obs - H.X(k+1|k)");
|
||
|
|
||
|
|
||
|
// X = X + K*DELTA
|
||
|
double KD[4][1];
|
||
|
double Xkk[4][1];
|
||
|
Mul_Mat_Mat(4,2,K,2,1,DELTA,KD);
|
||
|
Add_Mat_Mat(4,1,X,4,1,KD,X);
|
||
|
Plot_Mat(X," X(k+1|k+1) = X(k+1|k) + K.Delta = ");
|
||
|
Add_Mat_Mat(4,1,Xk,4,1,KD,Xkk);
|
||
|
Plot_Mat(Xkk," X(k+1|k+1) = X(k+1|k) + K.Delta = ");
|
||
|
|
||
|
// P = P - K*H*P;
|
||
|
// W = H*P
|
||
|
double W[2][4];
|
||
|
Mul_Mat_Mat(2,4,H,4,4,P,W);
|
||
|
// Y = K*W
|
||
|
// W = K*H
|
||
|
double W[4][4];
|
||
|
Mul_Mat_Mat(4,2,K,2,4,H,W);
|
||
|
// Y = W*P
|
||
|
double Y[4][4];
|
||
|
Mul_Mat_Mat(4,2,K,2,4,W,Y);
|
||
|
Mul_Mat_Mat(4,4,W,4,4,Pk,Y);
|
||
|
// P = P-Y
|
||
|
Sub_Mat_Mat(4,4,P,4,4,Y,P);
|
||
|
Plot_Mat(P," P(k+1|k+1) = P(k+1|k) - K.H.P(k+1|k) = ");
|
||
|
double Pkk[4][4];
|
||
|
Sub_Mat_Mat(4,4,Pk,4,4,Y,Pkk);
|
||
|
Plot_Mat(Pk," 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;
|
||
Formats disponibles : Unified diff
Commit Filtre Kalman pas complet