Travaux Pratiques » Polytech GE sp4a 2022

/** CELIK - Filtre kalman **/

int cpt = 0, i, j;

/**.....................Implantation des matrices intermediaires pour les iterations du filtre Kalman.....................**/

/* pour simplifier les iterations du filtre de Kalman : X1 = X(k+1|k) et X2 = X(k+1|k+1)

P1 = P(k+1|k) et P2 = P(k+1|k+1) */

double X1[4][1] = {{0},{0},{0},{0}};

double P1[4][4] = {{sigma_etat*sigma_etat, 0, 0, 0},

{0, sigma_etat*sigma_etat, 0, 0},

{0, 0, 0, 0},

{0, 0, 0, 0}};

double X2[4][1] = {{0},{0},{0},{0}};

double P2[4][4] = {{sigma_etat*sigma_etat, 0, 0, 0},

{0, sigma_etat*sigma_etat, 0, 0},

{0, 0, 0, 0},

{0, 0, 0, 0}};

/* Les resultats des fonctions creees precedemment (Add, Sub, Inv, Mul) seront stockes dans les matrices int[i][j] qui permettront une prise en main des iterations plus facilement */

double int1[4][4], int2[4][4], int3[4][4], int4[4][4], int5[4][4], int6[4][4], int7[4][4], int8[4][4], int9[4][4]; /*int10[4][4], int11[4][4], int12[4][4];*/

double delta[2][1]; /* delta = XY-H*X1 */

/**------------------------------------------------------------------------------------------------------------------------**/

while(fscanf(fichier, "%lf %lf %lf", &t, &xobs, &yobs)>0){ /* '&& cpt<3' qui permet de recuperer les 2 premieres iterations du filtre */

printf("-------------%04d--------------\n",cpt);

if (cpt ==0)

{

t0=t;x0=xobs;y0=yobs;

xobs=xobs-x0;yobs=yobs-y0;

Plot_Mat(F,"F = ");

Plot_Mat(H,"H = ");

Plot_Mat(R,"R = ");

}

else

{

t -= t0;xobs -= x0;yobs -= y0;

debug=0; ///Mettre à 1 pour afficher les matrices.

///Ajouter votre code ci-dessous///

// Kalman

double XY[2][1]={{xobs},{yobs}};

/** PREDICTION **/

// X = F*X

Mul_Mat_Mat(4, 4, F, 4, 1, X, X1); /* X(k+1|k) = F*X(k|k) */

Plot_Mat(X1," X(k+1|k) = ");

//P = F*P*F'+Q

Mul_Mat_Mat(4, 4, F, 4, 4, P, int1); /* int1 = F*P */

Mul_Mat_Mat(4, 4, int1, 4, 4, FT, int2); /* int2 = int1*FT, avec FT(F transposee) */

Add_Mat_Mat(4, 4, int2, 4, 4, Q, P1); /* P1 = int2+Q */

Plot_Mat(P1,"P(k+1|k) = F.P(k|k).FT + Q = ");

/** GAIN **/

// K = P*H' /( H*P*H' + R);

Mul_Mat_Mat(4, 4, P1, 4, 2, HT, int1); /* int1 = P1*HT, avec HT (H transposee) */

Mul_Mat_Mat(2, 4, H, 4, 4, P1, int2); /* int2 = H*P1 (denominateur) */

Mul_Mat_Mat(2, 4, int2, 4, 2, HT, int3); /* int3 = int2*HT */

Add_Mat_Mat(2, 2, int3, 2, 2, R, int6); /* int6 = int3+R */

Inverse_Mat_22(2, 2, int6, int5); /* inversion de int6 qu'on va stocker dans int5 */

Mul_Mat_Mat(4, 2, int1, 2, 2, int5, K); /* K = int1*int5 */

Plot_Mat(K,"K = ");

/** MISE A JOUR **/

//X = X + K*([xobs(i);yobs(i)]-H*X);

Mul_Mat_Mat(2, 4, H, 4, 1, X1, int2); /* int2 = H*X1 */

Sub_Mat_Mat(2, 1, XY, 2, 1, int2, delta); /* delta = XY-int2, avec delta qui est la difference entre l'observation et la projection du vecteur d'etat */

Mul_Mat_Mat(4, 2, K, 2, 1, delta, int7); /* int7 = K*delta */

Add_Mat_Mat(4, 1, X1, 4, 1, int7, X2); /* X2 = X1+int7 */

Plot_Mat(delta,"DELTA = Obs - H.X(k+1|k)");

Plot_Mat(X2," X(k+1|k+1) = X(k+1|k) + K.Delta = ");

// P = P - K*H*P;

Mul_Mat_Mat(4, 2, K, 2, 4, H, int8); /* int8 = H*K */

Mul_Mat_Mat(4, 4, int8, 4, 4, P1, int9); /* int9 = int8*P1 */

Sub_Mat_Mat(4, 4, P1, 4, 4, int9, P2); /* P2 = P1*int9 */

Plot_Mat(P2," P(k+1|k+1) = P(k+1|k) - K.H.P(k+1|k) = ");

// Iteration de X(k|k) qui va devenir X(k+1|k) et celle de X(k+1|k) qui va devenir X(k+1|k+1)

for (i=0; i<4; i++)

{

for (j=0; j<1; j++)

{

X[i][j]=X2[i][j];

}

}

// Meme principe : iteration P(k|k) qui devient P(k+1|k) et P(k+1|k) qui devient P(k+1|k+1)

for (i=0; i<4; i++)

{

for (j=0; j<4; j++)

{

P[i][j]=P2[i][j];

}

}

/// La matrice X doit contenir la position filtrée ///

}

t = cpt * dt;

dx = (xobs - oldx)/dt;

dy = (yobs - oldy)/dt;

fprintf(Fout,"%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\n",t,xobs,yobs,sqrt(dx*dx+dy*dy)*dt,X[0][0],X[1][0],X[2][0],X[3][0],sqrt(X[2][0]*X[2][0]+X[3][0]*X[3][0])*dt);

oldx = xobs;

oldy = yobs;

cpt ++;

}

fclose(Fout);

fclose(fichier);

system ("gnuplot -p -e \"plot 'output.dat' u 5:6 w l, '' u 2:3 w l\";");

system ("gnuplot -p -e \"plot 'output.dat' u 1:9 w l, '' u 1:4 w l\";");

system ("gnuplot -p -e \"plot 'output.dat' u 9 w l , 'vitesse_reelle.dat' u 2 w l\";");

return 0;

}