Travaux Pratiques » Polytech GE sp4a 2022

void Add_Mat_Mat(int na,int ma,double A[na][ma],int nb,int mb,double B[nb][mb], double R[na][ma]){

int i, j; // variables permettant à parcourir l'ensembles des deux matrices en lignes et colonnes

for (j=0; j<ma; j++) //Boucle imbriquées nous permettant d'initialiser nos matrices( tableau) en lignes et en colonnes.

R[i][j] = A[i][j]+B[i][j]; // Addition de matrices se fait termes à termes

}

// Le determiannt est une valeur importante pour le calcul d'inverse de matrices 2*2

double det_A; // déclaration de la variable nous permettant le calcul du determinant de la matrice A.

det_A= A[0][0]*A[1][1]-A[1][0]*A[0][1]; // calcul du determinant de A, det = 1/ ad-cb pour une matrice carre 2x2 A= [a b , c d ]

// ici nous calculons l'inverse de A qui est égale à: A^-1= 1/(det A) * [d -c , -b a]

B[0][0]= A[1][1]*(1/(det_A));

B[0][1]= -A[0][1]*(1/(det_A));

B[1][0]= -A[1][0]*(1/(det_A));

B[1][1]= A[0][0]*(1/(det_A));

void Transpose_Mat(int n,int m,double A[n][m],double R[m][n]){

int i,j; // Deux oéprandes qui serviront à parcourir l'ensembles des deux matrices en lignes et colonne

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

for (j=0;j<m;j++) //Boucle imbriquées nous permettant d'initialisée nos matrices( tableau) en lignes et en colonnes.

R[j][i]=A[i][j]; // Inverse les lignes en colennes et inverssement.

}

void Sub_Mat_Mat(int na,int ma,double A[na][ma],int nb,int mb,double B[nb][mb], double R[na][ma]){

int i, j; // Deux oéprandes qui serviront à parcourir l'ensembles des deux matrices en lignes et colonne

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

for (j=0; j<ma; j++) //Boucle imbriquées nous permettant d'initialisée nos matrices( tableau) en lignes et en colonnes.

R[i][j] = A[i][j]-B[i][j];

}

void Mul_Mat_Mat(int na,int ma,double A[na][ma], int nb,int mb,double B[nb][mb], double R[na][mb]){

}

double xobs,yobs;

Transpose_Mat(4,4,F,FT);

while(fscanf(fichier, "%lf %lf %lf", &t, &xobs, &yobs)>0){

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

// X = F*X

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

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

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

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

Plot_Mat(K,"K = ");

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

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

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

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

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

/// 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;

}