Révision 360
Ajouté par Faty MBAYE il y a presque 4 ans
| branch/mbaye/sp4a3/sp4a3_kalman.c | ||
|---|---|---|
|
// Kalman
|
||
|
|
||
|
// X = F*X
|
||
|
double X1[4][1];
|
||
|
Mul_Mat_Mat(4,4,F,4,1,X,X1); // F*X=X1
|
||
|
Plot_Mat(X," X(k+1|k) = ");
|
||
|
Plot_Mat(P,"P(k|k) = ");
|
||
| ... | ... | |
|
double J[2][4];
|
||
|
double J1[2][4];
|
||
|
double J2[2][2];
|
||
|
double J3[2][2];
|
||
|
double J3[4][2];
|
||
|
double J4[4][2];
|
||
|
Mul_Mat_Mat(2,4,H,4,4,P,J); //H*P=J
|
||
|
Mul_Mat_Mat(2,4,J,4,2,HT,J1); //J*H'=J1
|
||
|
Add_Mat_Mat(2,4,J1,2,2,R,J2); //J1+R=J2
|
||
|
Add_Mat_Mat(2,2,J1,2,2,R,J2); //J1+R=J2
|
||
|
Inverse_Mat_22(2,2,J2,J3); //J2'=J3
|
||
|
|
||
|
Mul_Mat_Mat(4,4,P,4,2,HT,J4); //P*H'=J4
|
||
Formats disponibles : Unified diff
modif_kalman