Index: branch/sacco/sp4a3/sp4a3_kalman.c
===================================================================
--- branch/sacco/sp4a3/sp4a3_kalman.c	(revision 286)
+++ branch/sacco/sp4a3/sp4a3_kalman.c	(revision 287)
@@ -145,8 +145,10 @@
 
 }
 
-int main(int argc,char **argv){
+int main(int argc,char **argv){
+
 
+
 	tests_unitaires();
 
 	FILE* fichier = fopen("pos_t_x_y.dat","r");
@@ -164,6 +166,8 @@
 	double oldx,oldy;
 	double dx=0,dy=0,dt=0.1;
 	int cpt = 0;
+
+
 
 	// kalman param
 	double sigma_etat = 10.0;
@@ -196,24 +200,53 @@
 	double FT[4][4];
 	Transpose_Mat(4,4,F,FT);
 
-	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_44[4][4] = {{0,0,0,0},
+                              {0,0,0,0},
+                              {0,0,0,0},
+                              {0,0,0,0}};
 
-	double tampon_21[2][1];
-	double DELTA[2][1];
+	double tampon_44bis[4][4] = {{0,0,0,0},
+                                 {0,0,0,0},
+                                 {0,0,0,0},
+                                 {0,0,0,0}};
 
-	double Xcopie[4][1];
-	double Pcopie[4][4];
+	double tampon_24[2][4]= {{0,0,0,0},
+                             {0,0,0,0}};
+
+	double tampon_22[2][2] = {{0,0},{0,0}};
+	double tampon_22bis[2][2] = {{0,0},{0,0}};;
+	double tampon_22ter[2][2] = {{0,0},{0,0}};
+	double tampon_42[4][2]= {{0,0},
+                             {0,0},
+                             {0,0},
+                             {0,0}};
+	double tampon_41[4][1] = {{0},
+                              {0},
+                              {0},
+                              {0}};
+
+	double tampon_21[2][1] = {{0}, {0}};
+	double DELTA[2][1] = {{0}, {0}};
+
+	double Xcopie[4][1] = {{0},
+                           {0},
+                           {0},
+                           {0}};
+
+	double Pcopie[4][4] = {{0,0,0,0},
+                           {0,0,0,0},
+                           {0,0,0,0},
+                           {0,0,0,0}};
+
+    double Obs[2][1]={{0},{0}};
 
-	while(fscanf(fichier, "%lf %lf %lf", &t, &x, &y)>0){
-		printf("-------------%04d--------------\n",cpt);
+	while(fscanf(fichier, "%lf %lf %lf", &t, &x, &y)>0){
+
 
+		printf("-------------%04d--------------\n",cpt);
+
+
+
 		if (cpt ==0)
 		{
 			t0=t;x0=x;y0=y;
@@ -224,24 +257,36 @@
 		}
 		else
 		{
-			t -= t0;x -= x0;y -= y0;
-
-			debug=0; ///Mettre à 1 pour afficher les matrices.
+			t -= t0;x -= x0;y -= y0;
+
+			Obs[1][0] = y;
+			Obs[0][0]= x;
+
+			Xcopie[0][0] = X[0][0];
+			Xcopie[1][0] = X[1][0];
+			Xcopie[2][0] = X[2][0];
+			Xcopie[3][0] = X[3][0];
+
+
+
+            debug=0; ///Mettre à 1 pour afficher les matrices.
 			///Ajouter votre code ci-dessous///
-			// Kalman
+			// Kalman
 
 			// X = F*X
-			Mul_Mat_Mat(4,4,F, 4,1, X, tampon_41);
-			Plot_Mat(X," X(k+1|k) = ");
+			Mul_Mat_Mat(4,4,F, 4,1, X, Xcopie);
+			//Plot_Mat(X," X(k+1|k) = ");
 
 			//P = F*P*F'+Q;
 			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 = ");
+			//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, 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);
@@ -249,59 +294,40 @@
 			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 = ");
+			//Plot_Mat(K,"K = ");
 
+
 			//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 );
+			Add_Mat_Mat(4,1, Xcopie, 4,1, tampon_41, X );
 
 
-			//Plot_Mat(Delta,"DELTA = Obs - H.X(k+1|k)");
+			//Plot_Mat(DELTA,"DELTA = Obs - H.X(k+1|k)");
 
-            Plot_Mat(Xcopie," X(k+1|k+1) = X(k+1|k) + K.Delta = ");
+            //Plot_Mat(Xcopie," X(k+1|k+1) = X(k+1|k) + K.Delta = ");
 
 			// P = P - K*H*P;
 
+			for (int i=0; i<4;i++)
+            {
+                for(int j=0; j<4; j++)
+                {
+                    Pcopie[i][j] = P[i][j];
+                }
+            }
+
 			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 ///
+			Sub_Mat_Mat(4,4,Pcopie, 4,4, tampon_44bis, P);
+//            Plot_Mat(Pcopie," P(k+1|k+1) = P(k+1|k) - K.H.P(k+1|k) = ");
 
-			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[0][1] = Pcopie[0][1];
-			P[0][2] = Pcopie[0][2];
-			P[0][3] = Pcopie[0][3];
-
-			P[1][0] = Pcopie[1][0];
-			P[1][1] = Pcopie[1][1];
-			P[1][2] = Pcopie[1][2];
-			P[1][3] = Pcopie[1][3];
-
-			P[2][0] = Pcopie[2][0];
-			P[2][1] = Pcopie[2][1];
-			P[2][2] = Pcopie[2][2];
-			P[2][3] = Pcopie[2][3];
-
-			P[3][0] = Pcopie[3][0];
-			P[3][1] = Pcopie[3][1];
-			P[3][2] = Pcopie[3][2];
-			P[3][3] = Pcopie[3][3];
-
 
+
+			// La matrice X doit contenir la position filtrée //
+
 		}
 		t = cpt * dt;
 		dx = (x - oldx)/dt;
