Travaux Pratiques » Polytech GE sp4a 2022

#include <math.h>

int i,j;

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

{

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

{

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

//printf("%d ",R[i][j]);

}

//printf("\n");

}

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

double coef,det;

det=(A[0][0]*A[1][1])-(A[1][0]*A[0][1]);

coef=1/det;

R[0][0]=A[1][1]*coef;

R[1][0]=-A[1][0]*coef;

R[1][1]=A[0][0]*coef;

R[0][1]=-A[0][1]*coef;

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

{

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

{

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

//printf("%d ",R[j][i]);

}

//printf("\n");

}

int i,j;

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

{

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

{

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

//printf("%d ",R[i][j]);

}

//printf("\n");

}

int i,j,k;

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

{

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

{

R[i][j]=0;

for (k=0;k<ma;k++)

{

R[i][j]+=A[i][k]*B[k][j];

}

printf("%d ",R[i][j]);

}

printf("\n");

}

}

int main(int argc,char **argv){

tests_unitaires();

FILE* fichier = fopen("pos_t_x_y.dat","r");

if (fichier == NULL)

error("Impossible d'ouvrir le fichier GPGGA_data.dat");

FILE * Fout = Fout = fopen("output.dat","w");

if (fichier == NULL)

error("Impossible d'ouvrir le fichier output.dat");

printf("Kalman\n");

double t = 0;

double t0,x0,y0;

double xobs,yobs;

double oldx,oldy;

double dx=0,dy=0,dt=0.1;

int cpt = 0,i,j;

// kalman param

double sigma_etat = 10.0;

double sigma_observation = 2.0;

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

double P[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 Q[4][4] = {{0, 0, 0, 0},

{0, 0, 0, 0},

{0, 0, 0.1, 0},

{0, 0, 0, 0.1}};

double R[2][2] = {{sigma_observation*sigma_observation, 0},

{0 , sigma_observation*sigma_observation}};

double K[4][2];

double H[2][4] = {{1, 0, 0, 0},

{0, 1, 0, 0}};

double HT[4][2];

Transpose_Mat(2,4,H,HT);

double F[4][4] = {{1, 0, dt, 0},

{0, 1, 0, dt},

{0, 0, 1, 0},

{0, 0, 0, 1}};

double FT[4][4];

Transpose_Mat(4,4,F,FT);

//variables intermediaires

double FX[4][1];

double FP[4][4];

double Ftrans[4][4];

double FPFtrans[4][4];

double FPFtransPlusQ[4][4];

double Htrans[4][2];

double PHtrans[4][2];

double HP[2][4];

double HPHtrans[2][2];

double HPHtransPlusR[2][2];

double InvHPHtransPlusR[2][2];

double PHtransInvHPHtransPlusR[4][2];

double HFX[2][1];

double ObsMoinsHFX[2][1];

double KObsMoinsHFX[4][1];

double FXPlusKObsMoinsHFX[4][1];

double KH[4][4];

double KHP[4][4];

double PMoinsKHP[4][4];

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

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

{

int i,j;

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

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

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

// Kalman

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

// X = F*X

Mul_Mat_Mat(4,4,F,4,1,X,FX);

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

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

Mul_Mat_Mat(4,4,F,4,4,P,FP);

Transpose_Mat(4,4,F,Ftrans);

Mul_Mat_Mat(4,4,FP,4,4,Ftrans,FPFtrans);

Add_Mat_Mat(4,4,FPFtrans,4,4,Q,FPFtransPlusQ);

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

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

Transpose_Mat(2,4,H,Htrans);

Mul_Mat_Mat(4,4,FPFtransPlusQ,4,2,Htrans,PHtrans);

Mul_Mat_Mat(2,4,H,4,4,FPFtransPlusQ,HP);

Mul_Mat_Mat(2,4,HP,4,2,Htrans,HPHtrans);

Add_Mat_Mat(2,2,HPHtrans,2,2,R,HPHtransPlusR);

Inverse_Mat_22(2,2,HPHtransPlusR,InvHPHtransPlusR);

Mul_Mat_Mat(4,2,PHtrans,2,2,InvHPHtransPlusR,PHtransInvHPHtransPlusR);

Plot_Mat(PHtransInvHPHtransPlusR,"K = ");

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

Mul_Mat_Mat(2,4,H,4,1,FX,HFX);

Sub_Mat_Mat(2,1,Obs,2,1,HFX,ObsMoinsHFX);

Mul_Mat_Mat(4,2,PHtransInvHPHtransPlusR,2,1,ObsMoinsHFX,KObsMoinsHFX);

Add_Mat_Mat(4,1,FX,4,1,KObsMoinsHFX,FXPlusKObsMoinsHFX);

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

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

Mul_Mat_Mat(4,2,PHtransInvHPHtransPlusR,2,4,H,KH);

Mul_Mat_Mat(4,4,KH,4,4,FPFtransPlusQ,KHP);

Sub_Mat_Mat(4,4,FPFtransPlusQ,4,4,KHP,PMoinsKHP);

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

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

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

{

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

{

X[i][j]=FXPlusKObsMoinsHFX[i][j];

}

}

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

{

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

{

P[i][j]=PMoinsKHP[i][j];

}

}

}

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;

}