Révision 466
Ajouté par malameynar il y a presque 4 ans
| sp4a3_kalman.c | ||
|---|---|---|
|
double DELTA[2][1];
|
||
|
double C1 [2][4];
|
||
|
double C2[4][1];
|
||
|
double C3[4][4];
|
||
|
double C3[4][4];
|
||
|
double C4[2][2];
|
||
|
double C5[2][2];
|
||
|
Transpose_Mat(4,4,F,FT);
|
||
|
|
||
|
while(fscanf(fichier, "%lf %lf %lf", &t, &x, &y)>0){
|
||
| ... | ... | |
|
|
||
|
// K = P*H' / ( H*P*H' + R);
|
||
|
Transpose_Mat(2,4,H,HT);
|
||
|
Mul_Mat_Mat(4,4,P,4,2,HT,K);
|
||
|
Mul_Mat_Mat(4,4,P,4,2,HT,K);
|
||
|
Mul_Mat_Mat(2,4,H,4,2,K,C5);
|
||
|
Add_Mat_Mat(2,2,C5,2,2,R,C4);
|
||
|
Inverse_Mat_22(2,2,C4,C4);
|
||
|
Mul_Mat_Mat(4,2,K,2,2,C4,K);
|
||
|
|
||
|
|
||
|
Plot_Mat(K,"K = ");
|
||
|
|
||
|
//X = X + K*([xb(i);yb(i)]-H*X);
|
||
Formats disponibles : Unified diff
fin kalman