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]){ //Focntion réalisant l'addition de deux matrices carrée

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

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

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

void Inverse_Mat_22(int n,int m,double A[n][m],double B[n][m]){ // fonction qui calcul l'inverse d'une matrice carrée 2x2. n et m devront être en toute rigueur égal à 2

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

void Transpose_Mat(int n,int m,double A[n][m],double R[m][n]){ // fonction qui calcul la tranqposée d'une matrice carrée mxn

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

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 out[na][ma]){ //

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

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

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

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

out[i][j]=0; // initialisation de de chaque valeurs du tableau à zero avant le remplissage.

out[i][j] += A[i][k]*B[k][j]; // réalisation de la multiplication matricielle des lignes par des colonnes.

Transpose_Mat(4,4,F,FT);

double D[2][4];

double C[2][2];

double E[4][2];

double I[2][4];

double G[4][4];

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

t -= t0; x -= x0; y -= y0;

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;

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

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

Mul_Mat_Mat(2,4,D, 4,2,HT,C);

Add_Mat_Mat(2,2,C,2,2,R,C);

Inverse_Mat_22(2,2,C,E);

Mul_Mat_Mat(4,2,HT, 2,2,C,E);

Mul_Mat_Mat(4,4,P, 4,2,E,K);

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

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

Sub_Mat_Mat(4,4,P,4,4,G,P);

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

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

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

Mul_Mat_Mat(4,2,K, 2,4,I,G);

Sub_Mat_Mat(4,4,P,4,4,G,P);