Index: branch/pelloux/sp4a12/main.c
===================================================================
--- branch/pelloux/sp4a12/main.c	(revision 278)
+++ branch/pelloux/sp4a12/main.c	(revision 279)
@@ -15,6 +15,31 @@
                     "$GPZDA,141914.00,01,02,2006,00,00*69",
                     0};
 
+typedef struct
+{
+    float latitude;
+    float longitude;
+} Position;
+
+typedef struct
+{
+    Position rpos;
+    float vitmax;
+}Zone;
+
+//Variables globales
+int nb_Zones = 4;
+
+
+float a =10;
+float b = (float) 1;
+float c = (float) 1/6;
+float d = (float) 1/60;
+float e = (float) 1/600;
+float f = (float) 1/6000;
+float g = (float) 1/60000;
+float h = (float) 1/600000;
+
 int trame_cmp(char *trame, char *type)
 {
     int i=0,res=1;
@@ -35,9 +60,11 @@
         res = c-48;
         if ((res>9)||(res<0))
         {
-            res=(-1);
+            return -1;
+        }else
+        {
+            return (res);
         }
-        return (res);
     }
 
 int decode_nombre (char ch[], int x)
@@ -100,21 +127,100 @@
 {
     static int cpt=0 ;
     cpt++;
-    if (trame_cmp(trame,"GPGGA"))
+    Position pos;
+    if (decode_trame(trame,&pos)==1)
     {
-        printf ("> %s\n",trame);
-    }
+        printf ("> %s\n lattitude:%f; Longitude:%f\n",trame,pos.latitude,pos.longitude);
+    }
+    cpt++;
 }
 
-void decode_trame(char *trame, pos)
+int decode_trame(char *trame, Position *p)
 {
-    tramdeg[7];
+    if (trame_cmp(trame, "GPGGA"))
+    {
+        int nb_Virgule = 0;
+        int i =0;
+        int k = 0;
+        int j =0;
+        char Lat[10];
+        char Long[12];
+        int debut_Lat;
+        int debut_Long;
 
+        while (trame[i] !='\0')
+        {
+            if (nb_Virgule ==2 && i!= 26)
+            {
+                Lat[k] = trame[i];
+                k++;
+            }
+            if (nb_Virgule ==4 && i!= 39)
+            {
+                Long[j] = trame[i];
+                j++;
+            }
+            if (trame[i] == ',')
+            {
+                nb_Virgule += 1;
+            }
+            i++;
+        }
+        p->latitude = conversion(Lat);
+        p->longitude = conversion(Long);
 
+        return 1;
+    }
+    else
+    {
+        return 0;
+    }
 
+}
 
+float calcule_distance(Position p_1, Position p_2)
+{
+    float rpt = 0.01745329251;
+    float distance;
+    float delta =p_1.longitude-p_2.longitude;
+    float dist = 6378.1*acos(sin(p_1.latitude*rpt)*sin(p_2.latitude*rpt)+ cos(p_1.latitude*rpt)*cos(p_2.latitude*rpt)*cos(delta*rpt));
+    return dist;
 }
 
+float calcule_vitesse(Position p_1, Position p_2)
+{
+    float vitesse;
+    float dist = calcule_distance(p_1,p_2);
+    vitesse = dist*3600;
+}
+
+int distance_a_la_plus_proche_zone(Position p, Zone r[], int nb_zones, float *d)
+{
+    if (nb_zones ==0)
+    {
+        return -1;
+    }
+    else
+    {
+        int i;
+        int curseur =0;
+        float distance_min = 10000;
+        float distance;
+        for (i=0;i<nb_zones; i++)
+    {
+        distance =calcule_distance(p, r[i].rpos);
+        if (distance <= distance_min)
+        {
+            distance_min = distance;
+            curseur = i;
+        }
+    }
+    *d = distance_min;
+    return curseur;
+    }
+
+}
+
 
 //Ajouter vos tests unitaires dans cette fonction.
 void tests_unitaires(void){
@@ -171,7 +277,24 @@
        printf ("Erreur Test unitaire conversion.\n");
         exit(-1);
     }
-
+
+    Position test;
+    if (decode_trame("$GPGSV,3,3,10,22,39,053,50,28,15,320,*7E", &test) != 0){
+        printf("Erreur test unitaire decode_trame.\n");
+        exit(-1);
+    }
+    if (decode_trame("$GPGGA,141914.00,4545.0000,N,00306.6036,E,1,05,3.4,499.3,M,,M,,*7D", &test)!=1){
+        printf("Erreur test unitaire decode_trame.\n");
+        exit(-1);
+    }
+    if (decode_trame("$GPGGA,141925.00,4545.2410,N,00306.6046,E,1,05,3.4,501.4,M,,M,,*7D", &test)!=1){
+        printf("Erreur test unitaire decode_trame.\n");
+        exit(-1);
+    }
+    if (decode_trame("$GPRMC,141920.00,A,4545.6419,N,00306.6039,E,0.2,133.1,010206,,*38", &test) != 0){
+        printf("Erreur test unitaire decode_trame.\n");
+        exit(-1);
+    }
 }
 
 // Ne pas modifier cette fonction
Index: branch/pelloux/sp4a3/sp4a3_kalman.c
===================================================================
--- branch/pelloux/sp4a3/sp4a3_kalman.c	(revision 278)
+++ branch/pelloux/sp4a3/sp4a3_kalman.c	(revision 279)
@@ -7,11 +7,33 @@
 #include "sp4a3_kalman_extra.h"
 
 
-void Add_Mat_Mat(int na,int ma,double A[na][ma],int nb,int mb,double b[nb][mb], double out[na][ma]){
+void Add_Mat_Mat(int na,int ma,double A[na][ma],int nb,int mb,double b[nb][mb], double out[na][ma]){
+    int i=0;
+    int j=0;
+    if (na==nb && ma==mb)
+    {
+        for(i=0;i<=na-1;i++)
+        {
+            for(j=0;j<=ma-1;j++)
+            {
+                out[i][j] = A[i][j]+b[i][j];
+
+            }
+
+        }
+
+    }
 
 }
 
 void Inverse_Mat_22(int n,int m,double A[n][m],double B[n][m]){
+    double kdet;
+    kdet=(double)1/((A[0][0]*A[1][1])-(A[0][1]*A[1][0]));
+
+    B[0][0]=kdet*A[1][1];
+    B[1][1]=kdet*A[0][0];
+    B[0][1]=-kdet*A[0][1];
+    B[1][0]=-kdet*A[1][0];
 
 }
 
@@ -23,15 +45,52 @@
 }
 
 void Sub_Mat_Mat(int na,int ma,double A[na][ma],int nb,int mb,double b[nb][mb], double out[na][ma]){
+    int i=0;
+    int j=0;
+    if (na==nb && ma==mb)
+    {
+        for(i=0;i<=na-1;i++)
+        {
+            for(j=0;j<=ma-1;j++)
+            {
+                out[i][j] = A[i][j]-b[i][j];
+
+            }
+
+        }
+
+    }
 
 }
 
-void Mul_Mat_Mat(int na,int ma,double A[na][ma], int nb,int mb,double B[nb][mb], double out[na][mb]){
+void Mul_Mat_Mat(int na,int ma,double A[na][ma], int nb,int mb,double B[nb][mb], double out[na][mb]){
+
+
+    if (ma==nb)
+    {
+        int i;
+        int j;
+        int k;
+        for(int i=0;i<na;i++)
+        {
+           for(int j=0;j<mb;j++)
+           {
+               out[i][j]=0;
+               for (int k=0; k<ma;k++)
+               {
+
+                   out[i][j] += A[i][k]*B[k][j];
+               }
+
+           }
+        }
+    }
 
-}
+}
 
 
 
+
 void tests_unitaires(void){
 	//Matrices d'entrée
 	double T21a[2][1]={{7},{-5}};
@@ -154,24 +213,66 @@
 		{
 			t -= t0;x -= x0;y -= y0;
 
-			debug=0; ///Mettre à 1 pour afficher les matrices.
+			debug=1; ///Mettre à 1 pour afficher les matrices.
 			///Ajouter votre code ci-dessous///
 			// Kalman
 
-			// X = F*X
-			Plot_Mat(X," X(k+1|k) = ");
+			// X = F*X
+			double Xf [4][1];
+			Mul_Mat_Mat(4,4,F,4,1,X,Xf);
+			Plot_Mat(Xf," 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);
+			//P = F*P*F'+Q;
+			double PP [4][4];
+			double PPPP [4][4];
+			double Pf [4][4];
+			Mul_Mat_Mat(4,4,F,4,4,P,PP);
+			Mul_Mat_Mat(4,4,PP,4,4,FT,PPPP);
+			Add_Mat_Mat(4,4,PPPP,4,4,Q,Pf);
+			Plot_Mat(Pf,"P(k+1|k) = F.P(k|k).FT + Q = ");
+
+			// K = P*H' / ( H*P*H' + R);
+			double PHT[4][2];
+			double sa [2][4];
+			double st [2][2];
+			double st2 [2][2];
+			double stt [2][2];
+
+
+			Mul_Mat_Mat(4,4,P,4,2,HT,PHT);
+
+            Mul_Mat_Mat(2,4,H,4,4,P,sa);
+			Mul_Mat_Mat(2,4,sa,4,2,HT,st);
+			Add_Mat_Mat(2,2,st,2,2,R,st2);
+
+			Transpose_Mat(2,2,st2,stt);
+
+			Mul_Mat_Mat(4,2,PHT,2,2,stt,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(Delta,"DELTA = Obs - H.X(k+1|k)");
+			double sd [2][1];
+			double se [2][1];
+			double sf [2][1];
+			double ps [2][1]={x,y};
+			double X2 [4][1];
+            Mul_Mat_Mat(2,4,H,4,1,X,sd);
+            Sub_Mat_Mat(2,1,ps,2,1,sd,se);
+            Mul_Mat_Mat(2,1,se,4,2,K,sf);
+            Add_Mat_Mat(2,1,sf,4,1,X,X);
+
             Plot_Mat(X," X(k+1|k+1) = X(k+1|k) + K.Delta = ");
 
-			// P = P - K*H*P;
+			// P = P - K*H*P;
+			double KHP [2][2];
+			double KHP2 [2][2];
+			double P2 [4][4];
+            Mul_Mat_Mat(4,2,K,2,4,H,KHP);
+			Mul_Mat_Mat(2,2,KHP,4,4,P,KHP2);
+			Sub_Mat_Mat(4,4,P,2,2,KHP2,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 ///
