Index: branch/sacco/sp4a3/sp4a3_kalman.c
===================================================================
--- branch/sacco/sp4a3/sp4a3_kalman.c	(revision 275)
+++ branch/sacco/sp4a3/sp4a3_kalman.c	(revision 276)
@@ -119,7 +119,6 @@
 	double RST44[4][4]={{-11,-11,-7,-11},{-15,-17,-16,-32},{-69,-37,-5,3},{-49,-6,-9,-45}};
 	double RMT44T44[4][4]={{816,412,192,304},{1155,583,379,687},{1146,668,466,758},{486,308,222,338}};
 
-	double Y1[2][2] = {{5,2},{3,8}};
 
 
 	printf("Execution des tests unitaires.\n");
@@ -127,7 +126,7 @@
 	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");
 
-	//Add_Mat_Mat(2,2,T22b, 4,2, R22, R42);   if (Equal_Mat_Mat(RAT22, R42)    error("Erreur calcul Y1");
+
 	Add_Mat_Mat(2,2,T22a,2,2,T22b,R22);	if (!Equal_Mat_Mat(RAT22,R22)) 		error("Erreur calcul Addition 2x2");
 	Add_Mat_Mat(4,4,T44a,4,4,T44b,R44);	if (!Equal_Mat_Mat(RAT44,R44)) 		error("Erreur calcul Addition 4x4");
 	Add_Mat_Mat(4,1,T41a,4,1,T41b,R41);	if (!Equal_Mat_Mat(RAT41,R41)) 		error("Erreur calcul Addition 4x1");
@@ -197,9 +196,20 @@
 	double FT[4][4];
 	Transpose_Mat(4,4,F,FT);
 
-	double tampon_1[4][4];
-	double tampon_2[2][4];
+	double tampon_44[4][4];
+	double tampon_44bis[4][4];
+	double tampon_24[2][4];
+	double tampon_22[2][2];
+	double tampon_22bis[2][2];
+	double tampon_22ter[2][2];
+	double tampon_42[4][2];
+	double tampon_41[4][1];
 
+	double tampon_21[2][1];
+	double DELTA[2][1];
+
+	double Xcopie[4][1];
+	double Pcopie[4][4];
 
 	while(fscanf(fichier, "%lf %lf %lf", &t, &x, &y)>0){
 		printf("-------------%04d--------------\n",cpt);
@@ -221,34 +231,62 @@
 			// Kalman
 
 			// X = F*X
-			Mul_Mat_Mat(4,4,F, 4,1, X, X);
+			Mul_Mat_Mat(4,4,F, 4,1, X, tampon_41);
 			Plot_Mat(X," X(k+1|k) = ");
 
 			//P = F*P*F'+Q;
-			Mul_Mat_Mat(4,4,F,4,4,P,tampon_1);
-			Mul_Mat_Mat(4,4,tampon_1,4,4, FT, tampon_1);
-			Add_Mat_Mat(4,4,tampon_1,4,4,Q, P);
+			Mul_Mat_Mat(4,4,F,4,4,P,tampon_44);
+			Mul_Mat_Mat(4,4,tampon_44,4,4, FT, tampon_44bis);
+			Add_Mat_Mat(4,4,tampon_44bis,4,4,Q, P);
 			Plot_Mat(P,"P(k+1|k) = F.P(k|k).FT + Q = ");
 
 			// K = P*H' / ( H*P*H' + R);
 
-			Mul_Mat_Mat(4,4,P,4,2, HT, K);
-			Mul_Mat_Mat(2,4,H,4,4,P, tampon_2);
-			Mul_Mat_Mat(2,4,tampon_2, 4,2,HT, tampon_2);
-			Add_Mat_Mat(2,2,tampon_2,2,2,R, tampon_2);
-			Inverse_Mat_22(2,2,tampon_2, tampon_2);
-			Mul_Mat_Mat(4,2,K, 2,2, tampon_2, K);
+			Mul_Mat_Mat(4,4,P,4,2, HT, tampon_42);
+			Mul_Mat_Mat(2,4,H,4,4,P, tampon_24);
+			Mul_Mat_Mat(2,4,tampon_24, 4,2,HT, tampon_22);
+			Add_Mat_Mat(2,2,tampon_22,2,2,R, tampon_22bis);
+			Inverse_Mat_22(2,2,tampon_22bis, tampon_22ter);
+			Mul_Mat_Mat(4,2,tampon_42, 2,2, tampon_22ter, K);
 
 			Plot_Mat(K,"K = ");
 
-			//X = X + K*([xb(i);yb(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*([xb(i);yb(i)]-H*X);
+			double Obs[2][1];
+            Obs[0][0] = x;
+            Obs[1][0] = y;
+
+			Mul_Mat_Mat(2,4, H, 4,1, X, tampon_21);
+			Sub_Mat_Mat(2,1, Obs,2,1, tampon_21, DELTA);
+			Plot_Mat(Obs," Obs = [2][1] ");
+			Mul_Mat_Mat(4,2,K,2,1, DELTA,tampon_41);
+			Add_Mat_Mat(4,1, X, 4,1, tampon_41, Xcopie );
+
 
-			// 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(Delta,"DELTA = Obs - H.X(k+1|k)");
 
-			/// La matrice X doit contenir la position filtrée ///
+            Plot_Mat(Xcopie," X(k+1|k+1) = X(k+1|k) + K.Delta = ");
+
+			// P = P - K*H*P;
+
+			Mul_Mat_Mat(4,2, K, 2,4,H, tampon_44);
+			Mul_Mat_Mat(4,4,tampon_44, 4,4, P, tampon_44bis);
+			Sub_Mat_Mat(4,4,P, 4,4, tampon_44bis, Pcopie);
+            Plot_Mat(Pcopie," P(k+1|k+1) = P(k+1|k) - K.H.P(k+1|k) = ");
+
+			/// La matrice X doit contenir la position filtrée ///
+
+			X[0][0] = Xcopie[0][0];
+			X[1][0] = Xcopie[1][0];
+			X[2][0] = Xcopie[2][0];
+			X[3][0] = Xcopie[3][0];
+
+			P[0][0] = Pcopie[0][0];
+			P[1][0] = Pcopie[1][0];
+			P[2][0] = Pcopie[2][0];
+			P[3][0] = Pcopie[3][0];
+
+
 		}
 		t = cpt * dt;
 		dx = (x - oldx)/dt;
