Index: branch/MEMBRE/sp4a3/sp4a3_kalman.c
===================================================================
--- branch/MEMBRE/sp4a3/sp4a3_kalman.c	(revision 457)
+++ branch/MEMBRE/sp4a3/sp4a3_kalman.c	(revision 458)
@@ -15,7 +15,7 @@
         {
         for (int j = 0; j < mb; j++)
             {
-              R[i][ j]=A[i][j] + B[i][j];
+              R[i][ j]=A[i][j] + B[i][j]; /*Il additione termes à termes */
             }
         }
 }
@@ -27,11 +27,11 @@
 void Inverse_Mat_22(int n,int m,double A[n][m],double R[n][m])
 {
     int i,j;
-    int determinant = A[0][0]*A[1][1] - A[1][0]*A[0][1];
+    int determinant = A[0][0]*A[1][1] - A[1][0]*A[0][1]; /* calcul du determinant d'une matrice 2x2 */
     R[0][0]=A[1][1]/determinant;
     R[0][1]= - A[0][1]/determinant;
     R[1][0]= - A[1][0]/determinant;
-    R[1][1]=A[0][0]/determinant;
+    R[1][1]=A[0][0]/determinant; /* Utilisation de la formule d'inversion des matrices 2x2 pour calculer R,matrice inverse de A */
 
 }
 
@@ -46,7 +46,7 @@
     {
 		for (j=0;j<m;j++)
         {
-			R[j][i]=A[i][j];
+			R[j][i]=A[i][j]; /*transforme les lignes en colonnes */
         }
     }
 }
@@ -63,7 +63,7 @@
         {
             for (int j = 0; j < mb; j++)
             {
-              R[i][j] = A[i][j] - B[i][j];
+              R[i][j] = A[i][j] - B[i][j]; /* Soustrait termes à termes */
             }
         }
 }
@@ -83,7 +83,7 @@
                     R[i][j] = 0;
                     for (int l = 0; l < ma; l++)
                     {
-                        R[i][j] += A[i][l] * B[l][j];
+                        R[i][j] += A[i][l] * B[l][j]; /* R(i,j)=Sommes(A(i,l)*B(l,j) */
                     }
                 }
         }
@@ -198,6 +198,28 @@
 						 {0, 0, 0, 1}};
 	double FT[4][4];
 	Transpose_Mat(4,4,F,FT);
+
+
+
+
+            double R1[4][4];
+			double R2[4][4];
+            double R3[4][4];
+            double R4[4][2];
+            double R5[2][4];
+            double R6[2][2];
+            double R7[2][2];
+            double R8[4][2];
+            double R9[2][2];
+            double R10[4][4];
+            double R11[4][4];
+            double R15[4][1];
+            double R16[2][1];
+            double X2[4][1];
+            double P2[4][4];
+            double Delta[2][1];
+            double X1 [4][1];
+            int i,j;
 
 	while(fscanf(fichier, "%lf %lf %lf", &t, &xobs, &yobs)>0){
 		printf("-------------%04d--------------\n",cpt);
@@ -223,31 +245,23 @@
 			///Ajouter votre code ci-dessous///
 			// Kalman
 
-						double R1[4][4];
-			double R2[4][4];
-            double R3[4][4];
-            double R4[4][2];
-            double R5[2][4];
-            double R6[2][2];
-            double R7[2][2];
-            double R8[4][2];
-            double R9[2][2];
-            double R10[4][4];
-            double R11[4][4];
+
+            double Matriceobs[2][1]={{xobs},
+                                    {yobs}};
+
 
 			// X = F*X
-			Mul_Mat_Mat(4, 4, F, 4, 1, X, X);
-			Plot_Mat(X," X(k+1|k) = ");
+			Mul_Mat_Mat(4, 4, F, 4, 1, X, X1);
+			Plot_Mat(X1," X(k+1|k) = ");
+
 
 			//P = F*P*F'+Q;
-
-
-
 			Mul_Mat_Mat(4,4,F, 4,4,P, R1);
 			Transpose_Mat(4, 4, F, R2);
 			Mul_Mat_Mat(4, 4, R1, 4, 4, R2, R3);
 			Add_Mat_Mat(4, 4, Q, 4, 4, R3, P);
 			Plot_Mat(P,"P(k+1|k) = F.P(k|k).FT + Q = ");
+
 
 			// K = P*H' / ( H*P*H' + R);
 			Transpose_Mat(2, 4, H, R4);
@@ -258,16 +272,46 @@
 			Inverse_Mat_22(2,2,R7, R9);
 			Mul_Mat_Mat(4,2,R8, 2,2, R9, K);
 			Plot_Mat(K,"K = ");
+
+
+            //DELTA = Obs - H.X(k+1|k)
+            Mul_Mat_Mat(2,4,H,4,1,X1,R16);
+            Sub_Mat_Mat(2,1,Matriceobs, 2,1,R16,Delta);
+            Plot_Mat(Delta,"DELTA = Obs - H.X(k+1|k)");
+
+            //X2 = X + K*([xobs(i);yobs(i)]-H*X); revient à faire X+K*Delta
+			Mul_Mat_Mat(4,2,K,2,1,Delta,R15);
+			Add_Mat_Mat(4,1,X1,4,1,R15,X2);
+            Plot_Mat(X2," X(k+1|k+1) = X(k+1|k) + K.Delta = ");
 
-			//X = X + K*([xobs(i);yobs(i)]-H*X);
-			//Plot_Mat(Delta,"DELTA = Obs - H.X(k+1|k)");
-            Plot_Mat(X," X(k+1|k+1) = X(k+1|k) + K.Delta = ");
-
-			// P = P - K*H*P;
+			// P2 = P - K1*H*P;
 			Mul_Mat_Mat(4,2,K,2,4,H,R10);
 			Mul_Mat_Mat(4,4,R10,4,4,P,R11);
-			Sub_Mat_Mat(4,4,P,4,4,R11,P);
-            Plot_Mat(P," P(k+1|k+1) = P(k+1|k) - K.H.P(k+1|k) = ");
+			Sub_Mat_Mat(4,4,P,4,4,R11,P2);
+            Plot_Mat(P2," P(k+1|k+1) = P(k+1|k) - K.H.P(k+1|k) = ");
+
+            /*Mettre alors que P=P2 et X=X2 */
+
+
+            for (i = 0; i < 4; i++)
+            {
+                for (j = 0; j < 1; j++)
+                    {
+                        X[i][j]=X2[i][j];
+                    }
+            }
+
+
+            for (i = 0; i < 4; i++)
+            {
+                for (j = 0; j < 4; j++)
+                    {
+                        P[i][j]=P2[i][j];
+                    }
+            }
+
+
+
 
 			/// La matrice X doit contenir la position filtrée ///
 		}
