Travaux Pratiques » Polytech Ge SP4abc 2021

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]){

double d;

int i,j;

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

B[0][0]=A[1][1]*d;

B[1][1]=A[0][0]*d;

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

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

for (i=0;i<2;i++){

for (j=0;j<2;j++){

printf("%f \n",B[i][j]);

}

}

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

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

R[j][i]=A[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;

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

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

{

out[i][j]=A[i][0]*B[0][j];

for(k=1;k<ma;k++){

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

}

}

}

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

for (j=0;j<na;j++){

printf("%f \n",out[i][j]);

}

}

//Mul_Mat_Mat(4,4,T44a,4,1,T41a,R41); if (!Equal_Mat_Mat(RMT44T41,R41)) error("Erreur calcul Multiplication 4x4 4x1");

//Mul_Mat_Mat(4,4,T44a,4,2,T42a,R42); if (!Equal_Mat_Mat(RMT44T42,R42)) error("Erreur calcul Multiplication 4x4 4x2");

//Mul_Mat_Mat(4,2,T42a,2,1,T21a,R41); if (!Equal_Mat_Mat(RMT42T21,R41)) error("Erreur calcul Multiplication 4x2 2x1");

//Mul_Mat_Mat(4,2,T42a,2,2,T22a,R42); if (!Equal_Mat_Mat(RMT42T22,R42)) error("Erreur calcul Multiplication 4x2 2x2");

//Mul_Mat_Mat(4,2,T42a,2,4,T24,R44); if (!Equal_Mat_Mat(RMT42T24,R44)) error("Erreur calcul Multiplication 4x2 2x4");

//Mul_Mat_Mat(2,4,T24,4,1,T41a,R21); if (!Equal_Mat_Mat(RMT24T41,R21)) error("Erreur calcul Multiplication 2x4 4x1");

//Mul_Mat_Mat(2,4,T24,4,2,T42a,R22); if (!Equal_Mat_Mat(RMT24T42,R22)) error("Erreur calcul Multiplication 2x4 4x2");

//Mul_Mat_Mat(2,4,T24,4,4,T44a,R24); if (!Equal_Mat_Mat(RMT24T44,R24)) error("Erreur calcul Multiplication 2x4 4x4");

Transpose_Mat(2,4,T24,R42); if (!Equal_Mat_Mat(RTT24,R42)) error("Erreur calcul Transposition 2x4");

Transpose_Mat(4,4,T44a,R44); if (!Equal_Mat_Mat(RTT44,R44)) error("Erreur calcul Transposition 4x4");

//Inverse_Mat_22(2,2,T22a,R22); if (!Equal_Mat_Mat(RInvT22,R22)) error("Erreur calcul Inversion 2x2");

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

// X = F*X

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

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

double Ft[4][4];

Transpose_Mat(4,4,F,Ft);

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

Mul_Mat_Mat(4,4,P,4,4,Ft,P);

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

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

double Ht[4][2];

double S1[2][4];//Matrice intermédiaire de calcul

double S2[2][2];//Matrice intermédiaire de calcul

double S2t[2][2];//Matrice intermédiaire de calcul

Transpose_Mat(2,4,H,Ht);

Mul_Mat_Mat(4,4,P,4,2,Ht,K);//P*Ht

Mul_Mat_Mat(2,4,H,4,4,P,S1);//H*P

Mul_Mat_Mat(2,4,S1,4,2,Ht,S2);//H*P*Ht

Add_Mat_Mat(2,2,S2,2,2,R,S2);

Inverse_Mat_22(2,2,S2,S2t);

Mul_Mat_Mat(4,2,K,2,2,Ht,K);

//X = X + K*([xb(i);yb(i)]-H*X);

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

double S[2][1];//Matrice intermédiaire de calcul

double S0[4][2];

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

Sub_Mat_Mat(2,1,Obs,2,1,S,S);

Mul_Mat_Mat(4,2,K,2,1,S,S0);

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

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

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

double S5[4][4];

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

Mul_Mat_Mat(4,4,S5,4,4,P,S5);

Sub_Mat_Mat(4,4,P,4,4,S5,P);