Révision 572
Ajouté par Guillaume DAVID il y a presque 3 ans
| branch/david_guillaume/sp4a3/sp4a3_kalman.c | ||
|---|---|---|
|
/** P1 = F*P*Ft + Q **/
|
||
|
double Pint1[4][4],Pint2[4][4],P1[4][4];
|
||
|
|
||
|
Mul_Mat_Mat(4,4,P,4,4,FT,Pint1);
|
||
|
Mul_Mat_Mat(4,4,F,4,4,Pint1,Pint2);
|
||
|
Add_Mat_Mat(4,4,Pint1,4,4,Q,P1);
|
||
|
Mul_Mat_Mat(4,4,F,4,4,P,Pint1);
|
||
|
Mul_Mat_Mat(4,4,Pint1,4,4,FT,Pint2);
|
||
|
Add_Mat_Mat(4,4,Pint2,4,4,Q,P1);
|
||
|
|
||
|
Plot_Mat(P1,"P(k+1|k) = F.P(k|k).FT + Q = ");
|
||
|
|
||
|
/**< GAIN >**/
|
||
|
/** K = P1*Ht*inv(H*P1*Ht + R) **/
|
||
|
double Kint1[4][4],Kint2[4][4],Kint3[4][4],Kint4[4][4],Kint5[4][4];
|
||
|
double Kint1[2][4],Kint2[2][2],Kint3[2][2],Kint4[2][2],Kint5[4][2];
|
||
|
|
||
|
Mul_Mat_Mat(4,4,P1,4,2,HT,Kint1);
|
||
|
Mul_Mat_Mat(2,4,H,4,2,Kint1,Kint2);
|
||
|
Mul_Mat_Mat(2,4,H,4,4,P1,Kint1);
|
||
|
Mul_Mat_Mat(2,4,Kint1,4,2,HT,Kint2);
|
||
|
Add_Mat_Mat(2,2,Kint2,2,2,R,Kint3);
|
||
|
Inverse_Mat_22(2,2,Kint3,Kint4);
|
||
|
Mul_Mat_Mat(4,2,HT,2,2,Kint4,Kint5);
|
||
|
Mul_Mat_Mat(4,4,P1,4,2,Kint5,K);
|
||
|
Mul_Mat_Mat(4,4,P1,4,2,HT,Kint5);
|
||
|
Mul_Mat_Mat(4,2,Kint5,2,2,Kint4,K);
|
||
|
|
||
|
Plot_Mat(K,"K = ");
|
||
|
|
||
|
/**< MISE A JOUR >**/
|
||
|
/** D = Obs - H*X1 **/
|
||
|
double Dint1[2][4],D[2][1];
|
||
|
double Dint1[2][1],D[2][1];
|
||
|
|
||
|
double Obs[2][1]={{xobs},{yobs}};
|
||
|
|
||
| ... | ... | |
|
Plot_Mat(X11," X(k+1|k+1) = X(k+1|k) + K.Delta = ");
|
||
|
|
||
|
/** P11 = P1 - K*H*P1 **/
|
||
|
double P11int1[4][1],P11int2[4][1],P11[4][1];
|
||
|
double P11int1[4][4],P11int2[4][4],P11[4][4];
|
||
|
|
||
|
Mul_Mat_Mat(2,4,H,4,4,P1,P11int1);
|
||
|
Mul_Mat_Mat(4,2,K,2,4,P11int1,P11int2);
|
||
|
Mul_Mat_Mat(4,2,K,2,4,H,P11int1);
|
||
|
Mul_Mat_Mat(4,4,P11int1,4,4,P1,P11int2);
|
||
|
Sub_Mat_Mat(4,4,P1,4,4,P11int2,P11);
|
||
|
|
||
|
Plot_Mat(P11," P(k+1|k+1) = P(k+1|k) - K.H.P(k+1|k) = ");
|
||
Formats disponibles : Unified diff
commit final