Index: branch/CELIK/sp4a3/sp4a3_kalman.c
===================================================================
--- branch/CELIK/sp4a3/sp4a3_kalman.c	(revision 573)
+++ branch/CELIK/sp4a3/sp4a3_kalman.c	(revision 574)
@@ -1,4 +1,5 @@
 //  Pour compiler : gcc sp4a3_kalman.c  -lm
+/** CELIK - Filtre kalman **/
 
 #include <stdlib.h>
 #include <stdio.h>
@@ -143,7 +144,7 @@
 	double xobs,yobs;
 	double oldx,oldy;
 	double dx=0,dy=0,dt=0.1;
-	int cpt = 0;
+	int cpt = 0, i, j;
 
 	// kalman param
 	double sigma_etat = 10.0;
@@ -176,56 +177,125 @@
 	double FT[4][4];
 	Transpose_Mat(4,4,F,FT);
 
-	while(fscanf(fichier, "%lf %lf %lf", &t, &xobs, &yobs)>0){
-		printf("-------------%04d--------------\n",cpt);
-
-		if (cpt ==0)
-		{
-			t0=t;x0=xobs;y0=yobs;
-			xobs=xobs-x0;yobs=yobs-y0;
-			Plot_Mat(F,"F = ");
-			Plot_Mat(H,"H = ");
-			Plot_Mat(R,"R = ");
-		}
-		else
-		{
-			t -= t0;xobs -= x0;yobs -= y0;
-
-			debug=0; ///Mettre à 1 pour afficher les matrices.
-			///Ajouter votre code ci-dessous///
-			// Kalman
-
-			// X = F*X
-			Plot_Mat(X," X(k+1|k) = ");
-
-			//P = F*P*F'+Q;
-			Plot_Mat(P,"P(k+1|k) = F.P(k|k).FT + Q = ");
-
-			// K = P*H' / ( H*P*H' + R);
-			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 = ");
-
-			// P = P - K*H*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 ///
-		}
-		t = cpt * dt;
-		dx = (xobs - oldx)/dt;
-		dy = (yobs - oldy)/dt;
-		fprintf(Fout,"%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\n",t,xobs,yobs,sqrt(dx*dx+dy*dy)*dt,X[0][0],X[1][0],X[2][0],X[3][0],sqrt(X[2][0]*X[2][0]+X[3][0]*X[3][0])*dt);
-		oldx = xobs;
-		oldy = yobs;
-		cpt ++;
-	}
-	fclose(Fout);
-	fclose(fichier);
-
-	system ("gnuplot -p -e \"plot 'output.dat' u 5:6 w l, '' u 2:3 w l\";");
-	system ("gnuplot -p -e \"plot 'output.dat' u 1:9 w l, '' u 1:4 w l\";");
-	system ("gnuplot -p -e \"plot 'output.dat' u 9 w l  , 'vitesse_reelle.dat' u 2 w l\";");
-	return 0;
-}
+	/**.....................Implantation des matrices intermediaires pour les iterations du filtre Kalman.....................**/
+
+    /* pour simplifier les iterations du filtre de Kalman : X1 = X(k+1|k) et X2 = X(k+1|k+1)
+                                                  P1 = P(k+1|k) et P2 = P(k+1|k+1) */
+    double X1[4][1] = {{0},{0},{0},{0}};
+    double P1[4][4] = 	{{sigma_etat*sigma_etat, 0, 0, 0},
+                        {0, sigma_etat*sigma_etat, 0, 0},
+                        {0, 0, 0, 0},
+                        {0, 0, 0, 0}};
+
+    double X2[4][1] = {{0},{0},{0},{0}};
+    double P2[4][4] = 	{{sigma_etat*sigma_etat, 0, 0, 0},
+                        {0, sigma_etat*sigma_etat, 0, 0},
+                        {0, 0, 0, 0},
+                        {0, 0, 0, 0}};
+
+    /* Les resultats des fonctions creees precedemment (Add, Sub, Inv, Mul) seront stockes dans les matrices int[i][j] qui permettront une prise en main des iterations plus facilement */
+    double int1[4][4], int2[4][4], int3[4][4], int4[4][4], int5[4][4], int6[4][4], int7[4][4], int8[4][4], int9[4][4]; /*int10[4][4], int11[4][4], int12[4][4];*/
+
+    double delta[2][1]; /* delta = XY-H*X1 */
+
+    /**------------------------------------------------------------------------------------------------------------------------**/
+
+	while(fscanf(fichier, "%lf %lf %lf", &t, &xobs, &yobs)>0){  /* '&& cpt<3' qui permet de recuperer les 2 premieres iterations du filtre */
+		printf("-------------%04d--------------\n",cpt);
+
+		if (cpt ==0)
+		{
+			t0=t;x0=xobs;y0=yobs;
+			xobs=xobs-x0;yobs=yobs-y0;
+			Plot_Mat(F,"F = ");
+			Plot_Mat(H,"H = ");
+			Plot_Mat(R,"R = ");
+		}
+		else
+		{
+			t -= t0;xobs -= x0;yobs -= y0;
+
+			debug=0; ///Mettre Ã  1 pour afficher les matrices.
+			///Ajouter votre code ci-dessous///
+			// Kalman
+
+            double XY[2][1]={{xobs},{yobs}};
+
+            /** PREDICTION **/
+			// X = F*X
+			Mul_Mat_Mat(4, 4, F, 4, 1, X, X1);                                  /* X(k+1|k) = F*X(k|k) */
+			Plot_Mat(X1," X(k+1|k) = ");
+
+			//P = F*P*F'+Q
+			Mul_Mat_Mat(4, 4, F, 4, 4, P, int1);                                /* int1 = F*P */
+			Mul_Mat_Mat(4, 4, int1, 4, 4, FT, int2);                            /* int2 = int1*FT, avec FT(F transposee) */
+			Add_Mat_Mat(4, 4, int2, 4, 4, Q, P1);                               /* P1 = int2+Q */
+			Plot_Mat(P1,"P(k+1|k) = F.P(k|k).FT + Q = ");
+
+
+            /** GAIN **/
+			// K = P*H' /( H*P*H' + R);
+			Mul_Mat_Mat(4, 4, P1, 4, 2, HT, int1);                              /* int1 = P1*HT, avec HT (H transposee) */
+			Mul_Mat_Mat(2, 4, H, 4, 4, P1, int2);                               /* int2 = H*P1 (denominateur) */
+			Mul_Mat_Mat(2, 4, int2, 4, 2, HT, int3);                            /* int3 = int2*HT */
+			Add_Mat_Mat(2, 2, int3, 2, 2, R, int6);                             /* int6 = int3+R */
+
+			Inverse_Mat_22(2, 2, int6, int5);                                   /* inversion de int6 qu'on va stocker dans int5 */
+			Mul_Mat_Mat(4, 2, int1, 2, 2, int5, K);                             /* K = int1*int5 */
+			Plot_Mat(K,"K = ");
+
+
+			/** MISE A JOUR **/
+			//X = X + K*([xobs(i);yobs(i)]-H*X);
+            Mul_Mat_Mat(2, 4, H, 4, 1, X1, int2);                               /* int2 = H*X1 */
+            Sub_Mat_Mat(2, 1, XY, 2, 1, int2, delta);                           /* delta = XY-int2, avec delta qui est la difference entre l'observation et la projection du vecteur d'etat */
+			Mul_Mat_Mat(4, 2, K, 2, 1, delta, int7);                            /* int7 = K*delta */
+
+			Add_Mat_Mat(4, 1, X1, 4, 1, int7, X2);                              /* X2 = X1+int7 */
+			Plot_Mat(delta,"DELTA = Obs - H.X(k+1|k)");
+            Plot_Mat(X2," 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, int8);                                /* int8 = H*K */
+            Mul_Mat_Mat(4, 4, int8, 4, 4, P1, int9);                            /* int9 = int8*P1 */
+            Sub_Mat_Mat(4, 4, P1, 4, 4, int9, P2);                              /* P2 = P1*int9 */
+            Plot_Mat(P2," P(k+1|k+1) = P(k+1|k) - K.H.P(k+1|k) = ");
+
+
+            // Iteration de X(k|k) qui va devenir X(k+1|k) et celle de X(k+1|k) qui va devenir X(k+1|k+1)
+            for (i=0; i<4; i++)
+            {
+                for (j=0; j<1; j++)
+                {
+                    X[i][j]=X2[i][j];
+                }
+            }
+
+            // Meme principe : iteration P(k|k) qui devient P(k+1|k) et P(k+1|k) qui devient P(k+1|k+1)
+            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 ///
+		}
+		t = cpt * dt;
+		dx = (xobs - oldx)/dt;
+		dy = (yobs - oldy)/dt;
+		fprintf(Fout,"%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\n",t,xobs,yobs,sqrt(dx*dx+dy*dy)*dt,X[0][0],X[1][0],X[2][0],X[3][0],sqrt(X[2][0]*X[2][0]+X[3][0]*X[3][0])*dt);
+		oldx = xobs;
+		oldy = yobs;
+		cpt ++;
+	}
+	fclose(Fout);
+	fclose(fichier);
+
+	system ("gnuplot -p -e \"plot 'output.dat' u 5:6 w l, '' u 2:3 w l\";");
+	system ("gnuplot -p -e \"plot 'output.dat' u 1:9 w l, '' u 1:4 w l\";");
+	system ("gnuplot -p -e \"plot 'output.dat' u 9 w l  , 'vitesse_reelle.dat' u 2 w l\";");
+	return 0;
+}
