Révision 287
Ajouté par gasacco il y a environ 4 ans
| branch/sacco/sp4a3/sp4a3_kalman.c | ||
|---|---|---|
|
|
||
|
}
|
||
|
|
||
|
int main(int argc,char **argv){
|
||
|
int main(int argc,char **argv){
|
||
|
|
||
|
|
||
|
|
||
|
tests_unitaires();
|
||
|
|
||
|
FILE* fichier = fopen("pos_t_x_y.dat","r");
|
||
| ... | ... | |
|
double oldx,oldy;
|
||
|
double dx=0,dy=0,dt=0.1;
|
||
|
int cpt = 0;
|
||
|
|
||
|
|
||
|
|
||
|
// kalman param
|
||
|
double sigma_etat = 10.0;
|
||
| ... | ... | |
|
double FT[4][4];
|
||
|
Transpose_Mat(4,4,F,FT);
|
||
|
|
||
|
double tampon_44[4][4];
|
||
|
double tampon_44bis[4][4];
|
||
|
double tampon_24[2][4];
|
||
|
double tampon_22[2][2];
|
||
|
double tampon_22bis[2][2];
|
||
|
double tampon_22ter[2][2];
|
||
|
double tampon_42[4][2];
|
||
|
double tampon_41[4][1];
|
||
|
double tampon_44[4][4] = {{0,0,0,0},
|
||
|
{0,0,0,0},
|
||
|
{0,0,0,0},
|
||
|
{0,0,0,0}};
|
||
|
|
||
|
double tampon_21[2][1];
|
||
|
double DELTA[2][1];
|
||
|
double tampon_44bis[4][4] = {{0,0,0,0},
|
||
|
{0,0,0,0},
|
||
|
{0,0,0,0},
|
||
|
{0,0,0,0}};
|
||
|
|
||
|
double Xcopie[4][1];
|
||
|
double Pcopie[4][4];
|
||
|
double tampon_24[2][4]= {{0,0,0,0},
|
||
|
{0,0,0,0}};
|
||
|
|
||
|
double tampon_22[2][2] = {{0,0},{0,0}};
|
||
|
double tampon_22bis[2][2] = {{0,0},{0,0}};;
|
||
|
double tampon_22ter[2][2] = {{0,0},{0,0}};
|
||
|
double tampon_42[4][2]= {{0,0},
|
||
|
{0,0},
|
||
|
{0,0},
|
||
|
{0,0}};
|
||
|
double tampon_41[4][1] = {{0},
|
||
|
{0},
|
||
|
{0},
|
||
|
{0}};
|
||
|
|
||
|
double tampon_21[2][1] = {{0}, {0}};
|
||
|
double DELTA[2][1] = {{0}, {0}};
|
||
|
|
||
|
double Xcopie[4][1] = {{0},
|
||
|
{0},
|
||
|
{0},
|
||
|
{0}};
|
||
|
|
||
|
double Pcopie[4][4] = {{0,0,0,0},
|
||
|
{0,0,0,0},
|
||
|
{0,0,0,0},
|
||
|
{0,0,0,0}};
|
||
|
|
||
|
double Obs[2][1]={{0},{0}};
|
||
|
|
||
|
while(fscanf(fichier, "%lf %lf %lf", &t, &x, &y)>0){
|
||
|
printf("-------------%04d--------------\n",cpt);
|
||
|
while(fscanf(fichier, "%lf %lf %lf", &t, &x, &y)>0){
|
||
|
|
||
|
|
||
|
printf("-------------%04d--------------\n",cpt);
|
||
|
|
||
|
|
||
|
|
||
|
if (cpt ==0)
|
||
|
{
|
||
|
t0=t;x0=x;y0=y;
|
||
| ... | ... | |
|
}
|
||
|
else
|
||
|
{
|
||
|
t -= t0;x -= x0;y -= y0;
|
||
|
|
||
|
debug=0; ///Mettre à 1 pour afficher les matrices.
|
||
|
t -= t0;x -= x0;y -= y0;
|
||
|
|
||
|
Obs[1][0] = y;
|
||
|
Obs[0][0]= x;
|
||
|
|
||
|
Xcopie[0][0] = X[0][0];
|
||
|
Xcopie[1][0] = X[1][0];
|
||
|
Xcopie[2][0] = X[2][0];
|
||
|
Xcopie[3][0] = X[3][0];
|
||
|
|
||
|
|
||
|
|
||
|
debug=0; ///Mettre à 1 pour afficher les matrices.
|
||
|
///Ajouter votre code ci-dessous///
|
||
|
// Kalman
|
||
|
// Kalman
|
||
|
|
||
|
// X = F*X
|
||
|
Mul_Mat_Mat(4,4,F, 4,1, X, tampon_41);
|
||
|
Plot_Mat(X," X(k+1|k) = ");
|
||
|
Mul_Mat_Mat(4,4,F, 4,1, X, Xcopie);
|
||
|
//Plot_Mat(X," X(k+1|k) = ");
|
||
|
|
||
|
//P = F*P*F'+Q;
|
||
|
Mul_Mat_Mat(4,4,F,4,4,P,tampon_44);
|
||
|
Mul_Mat_Mat(4,4,tampon_44,4,4, FT, tampon_44bis);
|
||
|
Add_Mat_Mat(4,4,tampon_44bis,4,4,Q, P);
|
||
|
Plot_Mat(P,"P(k+1|k) = F.P(k|k).FT + Q = ");
|
||
|
//Plot_Mat(P,"P(k+1|k) = F.P(k|k).FT + Q = ");
|
||
|
|
||
|
// K = P*H' / ( H*P*H' + R);
|
||
|
|
||
|
|
||
|
|
||
|
Mul_Mat_Mat(4,4,P,4,2, HT, tampon_42);
|
||
|
Mul_Mat_Mat(2,4,H,4,4,P, tampon_24);
|
||
|
Mul_Mat_Mat(2,4,tampon_24, 4,2,HT, tampon_22);
|
||
| ... | ... | |
|
Inverse_Mat_22(2,2,tampon_22bis, tampon_22ter);
|
||
|
Mul_Mat_Mat(4,2,tampon_42, 2,2, tampon_22ter, K);
|
||
|
|
||
|
Plot_Mat(K,"K = ");
|
||
|
//Plot_Mat(K,"K = ");
|
||
|
|
||
|
|
||
|
//X = X + K*([xb(i);yb(i)]-H*X);
|
||
|
double Obs[2][1];
|
||
|
Obs[0][0] = x;
|
||
|
Obs[1][0] = y;
|
||
|
|
||
|
Mul_Mat_Mat(2,4, H, 4,1, X, tampon_21);
|
||
|
Sub_Mat_Mat(2,1, Obs,2,1, tampon_21, DELTA);
|
||
|
Plot_Mat(Obs," Obs = [2][1] ");
|
||
|
Mul_Mat_Mat(4,2,K,2,1, DELTA,tampon_41);
|
||
|
Add_Mat_Mat(4,1, X, 4,1, tampon_41, Xcopie );
|
||
|
Add_Mat_Mat(4,1, Xcopie, 4,1, tampon_41, X );
|
||
|
|
||
|
|
||
|
//Plot_Mat(Delta,"DELTA = Obs - H.X(k+1|k)");
|
||
|
//Plot_Mat(DELTA,"DELTA = Obs - H.X(k+1|k)");
|
||
|
|
||
|
Plot_Mat(Xcopie," X(k+1|k+1) = X(k+1|k) + K.Delta = ");
|
||
|
//Plot_Mat(Xcopie," X(k+1|k+1) = X(k+1|k) + K.Delta = ");
|
||
|
|
||
|
// P = P - K*H*P;
|
||
|
|
||
|
for (int i=0; i<4;i++)
|
||
|
{
|
||
|
for(int j=0; j<4; j++)
|
||
|
{
|
||
|
Pcopie[i][j] = P[i][j];
|
||
|
}
|
||
|
}
|
||
|
|
||
|
Mul_Mat_Mat(4,2, K, 2,4,H, tampon_44);
|
||
|
Mul_Mat_Mat(4,4,tampon_44, 4,4, P, tampon_44bis);
|
||
|
Sub_Mat_Mat(4,4,P, 4,4, tampon_44bis, Pcopie);
|
||
|
Plot_Mat(Pcopie," P(k+1|k+1) = P(k+1|k) - K.H.P(k+1|k) = ");
|
||
|
|
||
|
/// La matrice X doit contenir la position filtrée ///
|
||
|
Sub_Mat_Mat(4,4,Pcopie, 4,4, tampon_44bis, P);
|
||
|
// Plot_Mat(Pcopie," P(k+1|k+1) = P(k+1|k) - K.H.P(k+1|k) = ");
|
||
|
|
||
|
X[0][0] = Xcopie[0][0];
|
||
|
X[1][0] = Xcopie[1][0];
|
||
|
X[2][0] = Xcopie[2][0];
|
||
|
X[3][0] = Xcopie[3][0];
|
||
|
|
||
|
P[0][0] = Pcopie[0][0];
|
||
|
P[0][1] = Pcopie[0][1];
|
||
|
P[0][2] = Pcopie[0][2];
|
||
|
P[0][3] = Pcopie[0][3];
|
||
|
|
||
|
P[1][0] = Pcopie[1][0];
|
||
|
P[1][1] = Pcopie[1][1];
|
||
|
P[1][2] = Pcopie[1][2];
|
||
|
P[1][3] = Pcopie[1][3];
|
||
|
|
||
|
P[2][0] = Pcopie[2][0];
|
||
|
P[2][1] = Pcopie[2][1];
|
||
|
P[2][2] = Pcopie[2][2];
|
||
|
P[2][3] = Pcopie[2][3];
|
||
|
|
||
|
P[3][0] = Pcopie[3][0];
|
||
|
P[3][1] = Pcopie[3][1];
|
||
|
P[3][2] = Pcopie[3][2];
|
||
|
P[3][3] = Pcopie[3][3];
|
||
|
|
||
|
|
||
|
|
||
|
// La matrice X doit contenir la position filtrée //
|
||
|
|
||
|
}
|
||
|
t = cpt * dt;
|
||
|
dx = (x - oldx)/dt;
|
||
Formats disponibles : Unified diff
Filtre de Kalman fonctionnel à améliorer