Index: branch/GUIFO/sp4a12/main.c
===================================================================
--- branch/GUIFO/sp4a12/main.c	(revision 631)
+++ branch/GUIFO/sp4a12/main.c	(revision 632)
@@ -3,8 +3,9 @@
 #include <strings.h>
 #include "trame.h"
 #include "math.h"
-#define k 0.017453292519943
+#define k 111120
 #define N 200
+#define rads 0.01745329252
 
 //Trames de tests � modifier si n�cessaire.
 char * trames[]=  {"$GPGSV,3,2,10,15,03,077,,18,04,041,42,19,85,271,,20,08,214,*7C",
@@ -15,7 +16,8 @@
                     "$GPGSA,A,3,,03,,22,14,,01,,18,,,,3.9,3.4,1.9*39",
                     "$GPVTG,99.4,T,,M,0.4,N,0.7,K*57",
                     "$GPZDA,141914.00,01,02,2006,00,00*69",
-                    0};
+                    0};
+
 typedef struct {
     float latitude;
     float longitude;
@@ -62,7 +64,7 @@
     for(i=0;i<n;i++){
         valeur+=decode_int(c[i])*pow(10,n-1-i);
     }
-
+    return valeur;
 }
 //Conversion de la latitude et longitude en degree floatant
 float conversion(char *c)
@@ -120,10 +122,10 @@
             p.latitude=-p.latitude;
         }
 
-        if(indx_lat[0]=='W'){
+        if(indx_lon[0]=='W'){
             p.longitude=-p.longitude;
         }
-}
+    }
     else{
         d=0;
 
@@ -130,15 +132,40 @@
      }
     return d;
 }
-
+
+//Fonction qui calcule la distance entre deux positions
+float calcul_distance(Position p1,Position p2){
+    float x,a,y,z,distance,b;
+    a=(p2.longitude-p1.longitude);
+    b=(p1.latitude+p2.latitude)/2;
+    x=a*cos(b*rads);
+    y=p2.latitude-p1.latitude;
+    z=sqrt((x*x)+(y*y));
+    distance=k*z;
+    return distance;
+
+}
+
+//Fonction qui calcule la vitesse du vehicule par seconde
+/*float calcule_vitesse(Position p1, Position p2){
+
+    float distance,vitesse;
+    distance=calcul_distance(Position p1, Position p2);
+    vitesse=3.6*distance;
+    return vitesse;*
+
+}*/
 //Fonction � modifier !!!!!
 void traitement(char * trame)
 {
+    int d;
     static int cpt=0 ;
     cpt++ ;
     if (trame_cmp(trame,"GPGGA")==1)
     {
-       printf ("> %s\n",trame);
+    d=decode_trame(trame,Position p);
+        printf ("d= %d",d);
+
     }
 
 }
@@ -182,7 +209,7 @@
         printf ("Erreur Test unitaire basique decode nombre.\n");
         exit(-1);
     }
-       if (decode_nombre("632",2)!=63){
+       if (decode_nombre("400",2)!=40){
         printf ("Erreur Test unitaire basique decode nombre.\n");
         exit(-1);
     }
Index: branch/GUIFO/sp4a3/sp4a3_kalman.c
===================================================================
--- branch/GUIFO/sp4a3/sp4a3_kalman.c	(revision 631)
+++ branch/GUIFO/sp4a3/sp4a3_kalman.c	(revision 632)
@@ -185,8 +185,8 @@
 			Plot_Mat(X," X(k+1|k) = ");
 
 			//P = F*P*F'+Q;
-			double A0[4][4],A1[4][4],A2[4][2],A3[2][4],A4[2][4],B[2][2],D[2][2],C[2][2],E[2][2];
-            double delta[2][1],Obs[2][1];
+			double A0[4][4],A1[4][4],A2[4][2],A3[4][4],A4[4][4],B[2][2],D[2][2],C[2][2],E[2][1];
+            double delta[2][1],Obs[2][1],C1[4][1];
 			Mul_Mat_Mat(4,4,F,4,4,P,A0);
 			Mul_Mat_Mat(4,4,A0,4,4,FT,A1);
 			Add_Mat_Mat(4,4,A1,4,4,Q,P);
@@ -197,19 +197,23 @@
 			Mul_Mat_Mat(2,4,H,4,2,A2,B);
 			Add_Mat_Mat(2,2,B,2,2,R,C);
 			Inverse_Mat_22(2,2,C,D) ;
-            Mul_Mat_Mat(2,2,A2,2,2,D,K);
+            Mul_Mat_Mat(4,2,A2,2,2,D,K);
 			Plot_Mat(K,"K = ");
 
 			//X = X + K*([xb(i);yb(i)]-H*X);
 
 			//Plot_Mat(Delta,"DELTA = Obs - H.X(k+1|k)");
-
+
+            Mul_Mat_Mat(2,4,H,4,1,X,E);
+            Sub_Mat_Mat(2,1,Obs,2,1,E,delta);
+            Mul_Mat_Mat(4,2,K,2,1,delta,C1);
+            Add_Mat_Mat(4,1,X,4,1,C1,X);
             Plot_Mat(X," X(k+1|k+1) = X(k+1|k) + K.Delta = ");
 
 			// P = P - K*H*P;
-            Mul_Mat_Mat(2,2,K,2,4,H,A3);
-            Mul_Mat_Mat(2,4,A3,4,4,P,A4);
-            Sub_Mat_Mat(4,4,P,2,4,A4,P);
+            Mul_Mat_Mat(4,2,K,2,4,H,A3);
+            Mul_Mat_Mat(4,4,A3,4,4,P,A4);
+            Sub_Mat_Mat(4,4,P,4,4,A4,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 ///
