Révision 252
Ajouté par clbouillot2 il y a presque 4 ans
| sp4a3_kalman.c | ||
|---|---|---|
|
// Kalman
|
||
|
// X = F*X
|
||
|
double Res1 [4][1];
|
||
|
X=Mul_Mat_Mat(4,4,F,4,1,X,Res1);
|
||
|
Mul_Mat_Mat(4,4,F,4,1,X,Res1);
|
||
|
X=Copy_mat(Res1);
|
||
|
Plot_Mat(X," X(k+1|k) = ");
|
||
|
|
||
|
//P = F*P*F'+Q;
|
||
| ... | ... | |
|
|
||
|
//X = X + K*([xb(i);yb(i)]-H*X);
|
||
|
//Plot_Mat(Delta,"DELTA = Obs - H.X(k+1|k)");
|
||
|
double Res14 [2][1], Res15[4][1], Res16[?][?], Res17[?][?],HX[2][1],Vect[2][1], KVect[4][1],KVectHX[2][1];
|
||
|
double Res14 [2][1], Res15[4][1], Res16[2][1], Res17[?][?],HX[2][1],Vect[2][1], KVectHX[4][1],VectHX[2][1];
|
||
|
Vect[0][0]=x;
|
||
|
Vect[1][0]=y;
|
||
|
HX=Mul_Mat_Mat(2,4,H,4,1,X,Res14);
|
||
|
Vect=[x[i],y[i]];
|
||
|
KVect=Mul_Mat_Mat(4,2,K,2,1,Vect,Res15);
|
||
|
KVectHX=Sub_Mat_Mat(4,1,KVect,2,1,HX,Res16);
|
||
|
X=Add_Mat_Mat(4,1,X,2,1,KVectHX,Res17);
|
||
|
VectHX=Sub_Mat_Mat(2,1,Vect,2,1,HX,Res16);
|
||
|
KVectHX=Mul_Mat_Mat(4,2,K,2,1,VectHX,Res15);
|
||
|
X=Add_Mat_Mat(4,1,X,4,1,KVectHX,Res17);
|
||
|
|
||
|
Plot_Mat(X," X(k+1|k+1) = X(k+1|k) + K.Delta = ");
|
||
|
|
||
Formats disponibles : Unified diff
Manque l'ajout de la fonction Copy_mat