Travaux Pratiques » Polytech Ge SP4abc 2021

void initTampon(int na,int ma,double Tampon[na][ma],double recopie[na][ma])

{

int i,j;

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

{

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

{

Tampon[i][j]=recopie[i][j];

}

}

}

void Add_Mat_Mat(int na,int ma,double A[na][ma],int nb,int mb,double b[nb][mb], double out[na][ma])

{

int i,j;

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

{

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

{

out[i][j]=A[i][j]+b[i][j];

}

}

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

{

int i,j;

double Tampon[n][m],det=0;

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

Tampon[0][0]=A[1][1];

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

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

Tampon[1][1]=A[0][0];

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

{

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

{

B[i][j]=(1/det)*Tampon[i][j];

}

}

void Sub_Mat_Mat(int na,int ma,double A[na][ma],int nb,int mb,double b[nb][mb], double out[na][ma])

{

int i,j;

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

{

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

{

out[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 out[na][mb])

{

int i,j,k;

double somme;

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

{

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

{

somme = 0;

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

{

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

out[i][j]=somme;

}

}

}

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

// X = F*X

double TamponX[4][1];

initTampon(4,1,TamponX,X);

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

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

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

double TamponP[4][4],intermediaire1[4][4],intermediaire2[4][4], Ftranspose[4][4];

Transpose_Mat(4,4,F,Ftranspose);

initTampon(4,4,TamponP,P);

Mul_Mat_Mat(4,4,F,4,4,TamponP,intermediaire1);

Mul_Mat_Mat(4,4,intermediaire1,4,4,Ftranspose,intermediaire2);

Add_Mat_Mat(4,4,intermediaire2,4,4,Q, P);

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

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

double intermediaire3[4][2],intermediaire4[2][4],intermediaire5[2][2],denominateur[2][2],Htranspose[4][2],inverse[2][2];

Transpose_Mat(2,4,H,Htranspose);

Mul_Mat_Mat(4,4,P,4,2,Htranspose,intermediaire3);

Mul_Mat_Mat(2,4,H,4,4,P,intermediaire4);

Mul_Mat_Mat(2,4,intermediaire4,4,2,Htranspose,intermediaire5);

Add_Mat_Mat(2,2,intermediaire5,2,2,R, denominateur);

Inverse_Mat_22(2,2,denominateur,inverse);

Mul_Mat_Mat(4,2,intermediaire3,2,2,inverse,K);

Plot_Mat(K,"K = ");

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

double Vecteurobserv[2][1]={{x},{y}};

double intermediaire6[2][1],intermediaire7[2][1],intermediaire8[4][1];

Mul_Mat_Mat(2,4,H,4,1,X,intermediaire6);

Sub_Mat_Mat(2,1,Vecteurobserv,2,1,intermediaire6, intermediaire7);

Mul_Mat_Mat(4,2,K,2,1,intermediaire7,intermediaire8);

Add_Mat_Mat(4,1,X,4,1,intermediaire8, X);

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

double intermediaire9[4][4],intermediaire10[4][4];

Mul_Mat_Mat(4,2,K,2,4,H,intermediaire9);

Mul_Mat_Mat(4,4,intermediaire9,4,4,P,intermediaire10);

Sub_Mat_Mat(4,4,P,4,4,intermediaire10, P);