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