Index: branch/MBADINGA/sp4a3/sp4a3_kalman.c
===================================================================
--- branch/MBADINGA/sp4a3/sp4a3_kalman.c	(revision 357)
+++ branch/MBADINGA/sp4a3/sp4a3_kalman.c	(revision 358)
@@ -7,23 +7,25 @@
 #include "sp4a3_kalman_extra.h"
 
 
-void Add_Mat_Mat(int na,int ma,double A[na][ma],int nb,int mb,double b[nb][mb], double out[na][ma]){
+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;
+int i, j; // Deux oéprandes qui serviront à parcourir l'ensembles des deux matrices en lignes et colonnes
 for (i=0; i<na; i++)
-       for (j=0; j<ma; j++)
-              out[i][j] = A[i][j]+b[i][j];
+       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]){
+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
 
 
-double det_A;
+// 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];
+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));
@@ -31,17 +33,17 @@
 
 }
 
-void Transpose_Mat(int n,int m,double A[n][m],double R[m][n]){
-	int i,j;
+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 (i=0;i<n;i++)
-		for (j=0;j<m;j++)
-			R[j][i]=A[i][j];
+		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;
+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 (i=0; i<na; i++)
-       for (j=0; j<ma; j++)
+       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];
 
 }
@@ -48,14 +50,14 @@
 
 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;
+ 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++)
+ for (i=0; i<na; i++)       //Boucle imbriquées nous permettant d'initialisée nos matrices( tableau) en lignes et en colonnes.
      for (j=0; j<mb; j++)
          {
-          out[i][j]=0;
+          out[i][j]=0;     // initialisation de de chaque valeurs du tableau à zero avant le remplissage.
           for (k=0; k<ma; k++)
-               out[i][j] += A[i][k]*B[k][j];
+               out[i][j] += A[i][k]*B[k][j]; // réalisation de la multiplication matricielle des lignes par des colonnes.
          }
 }
 
@@ -166,7 +168,14 @@
 						 {0, 0, 1, 0},
 						 {0, 0, 0, 1}};
 	double FT[4][4];
-	Transpose_Mat(4,4,F,FT);
+	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}};
 
 	while(fscanf(fichier, "%lf %lf %lf", &t, &x, &y)>0){
 		printf("-------------%04d--------------\n",cpt);
@@ -181,26 +190,46 @@
 		}
 		else
 		{
-			t -= t0;x -= x0;y -= y0;
+			t -= t0; x -= x0; y -= y0;
 
-			debug=0; ///Mettre à 1 pour afficher les matrices.
+			debug=1; ///Mettre à 1 pour afficher les matrices.
 			///Ajouter votre code ci-dessous///
 			// Kalman
 
-			// X = F*X
+			// X = F*X
+			Mul_Mat_Mat(4,4,F, 4,1,X,X);
 			Plot_Mat(X," X(k+1|k) = ");
 
-			//P = F*P*F'+Q;
+			//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);
+
 			Plot_Mat(P,"P(k+1|k) = F.P(k|k).FT + Q = ");
 
-			// K = P*H' / ( H*P*H' + R);
+			// 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);
+
 			Plot_Mat(K,"K = ");
 
-			//X = X + K*([xb(i);yb(i)]-H*X);
+	  /*      //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(Delta,"DELTA = Obs - H.X(k+1|k)");
-            Plot_Mat(X," X(k+1|k+1) = X(k+1|k) + K.Delta = ");
+            Plot_Mat(X," X(k+1|k+1) = X(k+1|k) + K.Delta = "); */
+
 
-			// P = P - K*H*P;
+			// 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);
+
             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 ///
