Révision 457
Ajouté par rademagalh il y a presque 4 ans
| branch/DEMAGALHAES/sp4a3/sp4a3_kalman.c | ||
|---|---|---|
|
|
||
|
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;
|
||
|
// Pk = A*F' + Q = B+Q
|
||
|
double A[4][4];
|
||
|
double B[4][4];
|
||
|
// A = F*P
|
||
|
Mul_Mat_Mat(4,4,F,4,4,P,A);
|
||
|
// B = A*FT
|
||
|
double FT[4][4];
|
||
|
Transpose_Mat(4,4,F,FT);
|
||
|
Mul_Mat_Mat(4,4,A,4,4,FT,B);
|
||
|
// P = B+Q
|
||
|
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 = ");
|
||
|
double FT[4][4];
|
||
|
Transpose_Mat(4,4,F,FT);
|
||
|
Mul_Mat_Mat(4,4,A,4,4,FT,B);
|
||
|
Add_Mat_Mat(4,4,B,4,4,Q,P);
|
||
|
Plot_Mat(P,"P(k+1|k) = F.P(k|k).FT + Q = ");
|
||
|
|
||
|
|
||
|
/*-------------------------------*/
|
||
|
// K = P*H' / ( H*P*H' + R);
|
||
|
// K = C * /E
|
||
|
double C[4][2];
|
||
|
double HT[4][2];
|
||
|
Transpose_Mat(2,4,H,HT);
|
||
|
Mul_Mat_Mat(4,4,Pk,4,2,HT,C);
|
||
|
// P*H' = C
|
||
|
double HT[4][2];
|
||
|
Transpose_Mat(2,4,H,HT);
|
||
|
double C[4][2];
|
||
|
Mul_Mat_Mat(4,4,P,4,2,HT,C);
|
||
|
Plot_Mat(C,"P(k+1|k).HT = ");
|
||
|
|
||
|
double D[2][4];
|
||
|
Mul_Mat_Mat(2,4,H,4,4,Pk,D);
|
||
|
// H*P*H' + R
|
||
|
// H*C + R = E + R = E_add
|
||
|
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 = ");
|
||
|
Mul_Mat_Mat(2,4,H,4,2,C,E);
|
||
|
double E_add[2][2];
|
||
|
Add_Mat_Mat(2,2,E,2,2,R,E_add);
|
||
|
Plot_Mat(E_add,"H.P(k+1|k).HT + R = ");
|
||
|
|
||
|
// INV(H*P*H' + R)
|
||
|
double E_inv[2][2];
|
||
|
Inverse_Mat_Mat(2,2,E,E_inv);
|
||
|
Inverse_Mat_Mat(2,2,E_add,E_inv);
|
||
|
Plot_Mat(E_inv,"INV(H.P(k+1|k).HT + R) = ");
|
||
|
|
||
|
Mul_Mat_Mat(2,2,E_inv,4,2,C,K);
|
||
|
Mul_Mat_Mat(4,2,C,2,2,E_inv,K);
|
||
|
Plot_Mat(K,"K = ");
|
||
|
|
||
|
/*-------------------------------*/
|
||
|
double OBS[2][1]={{x},{y}};
|
||
|
Plot_Mat(OBS,"Obs = ");
|
||
|
|
||
|
double obs[2][1]={{x},{y}};
|
||
|
Plot_Mat(obs,"Obs = ");
|
||
|
// DELTA = obs - H*X
|
||
|
// H*X = G;
|
||
|
double G[2][1];
|
||
|
Mul_Mat_Mat(2,4,H,4,1,X,G);
|
||
|
double DELTA[2][1];
|
||
|
double Z[2][1];
|
||
|
Mul_Mat_Mat(2,4,H,4,1,Xk,Z);
|
||
|
Sub_Mat_Mat(2,1,obs,2,1,Z,DELTA);
|
||
|
Sub_Mat_Mat(2,1,OBS,2,1,G,DELTA);
|
||
|
Plot_Mat(DELTA,"DELTA = Obs - H.X(k+1|k)");
|
||
|
|
||
|
/*-------------------------------*/
|
||
|
// X = X + K*DELTA
|
||
|
// X = X + Z
|
||
|
|
||
|
// 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,Xk,4,1,KD,Xkk);
|
||
|
Plot_Mat(Xkk," X(k+1|k+1) = X(k+1|k) + K.Delta = ");
|
||
|
|
||
|
double Z[4][1];
|
||
|
Mul_Mat_Mat(4,2,K,2,1,DELTA,Z);
|
||
|
Add_Mat_Mat(4,1,X,4,1,Z,X);
|
||
|
Plot_Mat(X," X(k+1|k+1) = X(k+1|k) + K.Delta = ");
|
||
|
|
||
|
/*-------------------------------*/
|
||
|
// P = P - K*H*P;
|
||
|
// W = K*H
|
||
|
// P = P - W*P = P - V
|
||
|
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,4,W,4,4,Pk,Y);
|
||
|
// P = P-Y
|
||
|
double V[4][4];
|
||
|
Mul_Mat_Mat(4,4,W,4,4,P,V);
|
||
|
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) = ");
|
||
|
Sub_Mat_Mat(4,4,P,4,4,V,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;
|
||
Formats disponibles : Unified diff