Index: branch/BRAVO/sp4a3/sp4a3_kalman.c
===================================================================
--- branch/BRAVO/sp4a3/sp4a3_kalman.c	(revision 575)
+++ branch/BRAVO/sp4a3/sp4a3_kalman.c	(revision 576)
@@ -17,7 +17,7 @@
         {
             for(j=0;j<mb;j++)
             {
-                AddM=A[i][j] + B[i][j];
+                AddM=A[i][j] + B[i][j]; //On somme les élements qui se trouvent dans les mêmes positions
                 R[i][j]=AddM;
             }
         }
@@ -32,8 +32,10 @@
 {
     double Det;
 
-    Det=(A[0][0]*A[1][1])-(A[0][1]*A[1][0]);//Calcul du déterminant
-    R[0][0]=(1/Det)*A[1][1];
+    Det=(A[0][0]*A[1][1])-(A[0][1]*A[1][0]);//Calcul du déterminant: Det(A={(a,b);(c,d)}=(ad-cb)
+
+    //(A)^(-1)=(1/det(A))*{(d,-b),(-c,a)}
+    R[0][0]=(1/Det)*A[1][1];//La matrice résultat a le même nombre de ligne que la matrice A
     R[0][1]=-(1/Det)*A[0][1];
     R[1][0]=-(1/Det)*A[1][0];
     R[1][1]=(1/Det)*A[0][0];
@@ -66,7 +68,7 @@
         {
             for(j=0;j<mb;j++)
             {
-                SubM=A[i][j] - B[i][j];
+                SubM=A[i][j] - B[i][j];//On soustrait les élements qui se trouvent dans les mêmes positions
                 R[i][j]=SubM;
             }
         }
@@ -80,16 +82,16 @@
 void Mul_Mat_Mat(int na,int ma,double A[na][ma], int nb,int mb,double B[nb][mb], double R[na][mb])
 {
     int i,j,k;
-    if(ma==nb)
+    if(ma==nb)//Si le nombre de colonnes de la matrice A égale au nombre de ligne de la matrice B
     {
-        for (int i = 0; i < na; i++)
+        for (i = 0; i < na; i++)//La matrice résultat a le même nombre de ligne que la matrice A
         {
-            for (int j = 0; j < mb; j++)
+            for (j = 0; j < mb; j++)//La matrice résultat a le même nombre de colonne que la matrice B
             {
                 R[i][j] = 0;
-                for (int k = 0; k < ma; k++)
+                for (k = 0; k < ma; k++)
                 {
-                    R[i][j] += A[i][k] * B[k][j];
+                    R[i][j] += A[i][k] * B[k][j];//On somme les résultats de la multiplication des élements de chaque ligne de A par celles de chaque colonne de B
                 }
             }
         }
@@ -209,8 +211,8 @@
 	double FT[4][4];
 	Transpose_Mat(4,4,F,FT);
 
- double X_l[4][1],X_2[2][1],X_3[2][1],X_4[4][1],X_5[4][1];
- double P_1[4][4],P_2[4][4],P_3[4][4],P4[4][4],P5[4][4],P6[4][4];
+ double X_1[4][1],X_2[2][1],X_3[2][1],X_4[4][1],X_5[4][1];
+ double P_1[4][4],P_2[4][4],P_3[4][4],P_4[4][4],P_5[4][4],P_6[4][4];
  double K_1[4][2],K_2[2][2],K_3[2][2],K_4[2][2],K_5[4][2],K_6[4][2];
 
 
@@ -230,26 +232,68 @@
 			t -= t0;xobs -= x0;yobs -= y0;
 
 			debug=1; ///Mettre à 1 pour afficher les matrices.
-			///Ajouter votre code ci-dessous///
+			///Ajouter votre code ci-dessous///
+
 			// Kalman
+            double Mobservation[2][1] ={{xobs},{yobs}};
+			// X = F*X
+			Mul_Mat_Mat(4,4,F, 4,1,X, X_1); //Multiplication de F et X, résultat stocké dans X_1
+			Plot_Mat(X_1," X(k+1|k) = ");
 
-			// X = F*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_1);//P_1=F*P
+			Mul_Mat_Mat(4,4,P_1, 4,4, FT,P_2);//P_2=P_1*FT=
+			Add_Mat_Mat(4,4,P_2,4,4,Q, P_3);//P_3=P_2+Q
 			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_3,K_1);//K_1=H*P_3
+			Mul_Mat_Mat(2,4,K_1, 4,2, HT,K_2);//K_2=K1*HT
+			Add_Mat_Mat(2,2,K_2,2,2,R, K_3);//K_3=K_2+R
+			Inverse_Mat_22(2,2,K_3,K_4);//K_4=(K_3)^(-1)
+            Mul_Mat_Mat(4,4,P_3, 4,2, HT,K_5);//K_5=P_3*HT
+            Mul_Mat_Mat(4,2,K_5, 2,2, K_4,K_6);//K_6=K_5*K_4
+
 			Plot_Mat(K,"K = ");
 
-			//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 = ");
+			//X = X + K*([xobs(i);yobs(i)]-H*X);
+			Mul_Mat_Mat(2,4,H, 4,1, X_1,X_2);//X_2=H*X_1
+			Sub_Mat_Mat(2,1,Mobservation,2,1,X_2,X_3);//X_3=Mobservation-X_2
+
+			//Plot_Mat(Delta,"DELTA = Obs - H.X(k+1|k)");
+			Plot_Mat(X_3,"DELTA = Obs - H.X(k+1|k)");
+			Mul_Mat_Mat(4,2,K_6, 2,1,X_3,X_4);//X_4=K_6*X_3
+			Add_Mat_Mat(4,1,X_1,4,1,X_4,X_5);//X_5=X_1+X_4
 
-			// P = P - K*H*P;
-            Plot_Mat(P," P(k+1|k+1) = P(k+1|k) - K.H.P(k+1|k) = ");
+            Plot_Mat(X_5," X(k+1|k+1) = X(k+1|k) + K.Delta = ");
 
-			/// La matrice X doit contenir la position filtrée ///
+			// P = P - K*H*P;
+			Mul_Mat_Mat(4,2,K_6, 2,4, H,P_4);//P_4=K_6*H
+			Mul_Mat_Mat(4,4,P_4, 4,4, P_3,P_5);//P_5=P_4*P_3
+			Sub_Mat_Mat(4,4,P_3,4,4,P_5, P_6);//P_6=P_3-P_5
+
+            Plot_Mat(P_6," P(k+1|k+1) = P(k+1|k) - K.H.P(k+1|k) = ");
+
+			/// La matrice X doit contenir la position filtrée ///
+
+        int i, j;
+		for ( i = 0; i <4 ; i++)
+        {
+            for ( j = 0; j <1 ; j++)
+            {
+                X[i][j] = X_5[i][j];
+            }
+
+        }
+        for ( i = 0; i <4 ; i++)
+        {
+            for ( j = 0; j <4 ; j++)
+            {
+                P[i][j] = P_6[i][j];
+
+            }
+        }
+
 		}
 		t = cpt * dt;
 		dx = (xobs - oldx)/dt;
