Révision 425
Ajouté par anclaud il y a presque 4 ans
| sp4a3_kalman.c | ||
|---|---|---|
|
|
||
|
// P = P - K*H*P;
|
||
|
double mat_inter7[4][4], mat_inter8[4][4];
|
||
|
Mul_Mat_Mat(4,2,K,2,4,H,mat_inter7);
|
||
|
Mul_Mat_Mat(4,4,mat_inter7,4,4,P,mat_inter8);
|
||
|
Sub_Mat_Mat(4,4,P,4,4,mat_inter8,P);
|
||
|
Mul_Mat_Mat(4,2,K,2,4,H,mat_inter7); // K*H
|
||
|
Mul_Mat_Mat(4,4,mat_inter7,4,4,P,mat_inter8); // K*H*P
|
||
|
Sub_Mat_Mat(4,4,P,4,4,mat_inter8,P); // P = P - K*H*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 ///
|
||
Formats disponibles : Unified diff
Filtre Kalman terminé