Index: branch/lashermes/sp4a12/main.c
===================================================================
--- branch/lashermes/sp4a12/main.c	(revision 281)
+++ branch/lashermes/sp4a12/main.c	(revision 282)
@@ -14,10 +14,25 @@
                     "$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};
 
-int trame_cmp(char* trame,char* type)
+typedef struct // definition de la structure
 {
+    float latitude;
+    float longitude;
+} Position;
+
+typedef struct // permet de definir une zone
+{
+    Position rpos;
+    float vitmax;
+}Zone;
+
+Zone zones[] = {{{44.208904, -1.297033},50}, {{47.478419, -0.563},70}, {{46.401488, 6.59049},50}, {{48.856614, 2.3522219},90}};
+int nb_Zones = 4;
+
+int trame_cmp(char* trame,char* type) // permet de verifier qu'on a la trame "GPGGA"
+{
     int i=0;
     do
     {
@@ -30,9 +45,9 @@
     return 1;
 }
 
-int decode_int(char c)
+int decode_int(char c) // permet de decoder en decimal
 {
-    int carac;
+    unsigned int carac;
     carac=c-48;
     if(carac<0 || carac>9)
     {
@@ -41,6 +56,20 @@
     return carac;
 }
 
+//Fonction � modifier !!!!!
+void traitement(char * trame) // permet de reconaitre les trame GPGGA
+{
+    static int cpt=0;
+    Position pos;
+    cpt++;
+    if (trame_cmp(trame,"GPGGA")==1)
+    {
+        printf ("%s\nLatitude: %f; Longitude: %f\n\n", trame, pos.latitude, pos.longitude);
+    }
+}
+
+
+
 // Fonction convertissant la chaine de carac en float
 int decode_nombre(char * ch, int n)
 {
@@ -54,7 +83,7 @@
     return entier;
 }
 
-// Fonction pour convertir latitude en float
+// Fonction pour convertir latitude en float !!!!! Ne fonctionne pas a revoir
 float convlat (char * latitude)
 {
     float carac=0;
@@ -116,27 +145,90 @@
 
 }
 
-int decode_trame(char *trame ,  *p )
+int decode_trame(char *trame , Position *p )
 {
-    if (trame_cmp(trame,"GPGGA")); // si la bonne trame
+    if (trame_cmp(trame,"GPGGA")) // si la bonne trame
     {
+        int pos_virgule=0;
+        int i=0;
+        int j=0;
+        int k=0;
+        char Lat[10];
+        char Long[12];
 
+        while (trame[i] != '\0')
+        {
+            if(pos_virgule==2 && i !=26)
+            {
+                Lat[k]=trame[i];
+                k++;
+            }
+            if(pos_virgule==4 && i !=39)
+            {
+                Long[j]=trame[i];
+                j++;
+            }
+            if(trame[i]==',')
+            {
+                pos_virgule +=1;
+            }
+            i++;
+        }
 
+        p->latitude=convlat(Lat);
+        p->longitude=convlog(Long);
+
+        return 1;
     }
+    else
+    {
+        return 0;
+    }
 }
 
+//A refaire
 
+/*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;
+}
 
-
-//Fonction � modifier !!!!!
-void traitement(char * trame) // permet de reconaitre les trame GPGGA
+float calcule_vitesse(Position p_1, Position p_2)
 {
-    static int cpt=0;
-    cpt++;
-    if (trame_cmp(trame,"GPGGA")==1)
+    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)
     {
-        printf ("> %s\n",trame);
-    }
+        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.
@@ -144,7 +236,8 @@
     if (5!=5){
         printf ("Erreur Test unitaire basique.\n");
         exit(-1);
-    }
+    }
+    //Tests unitaires de trame_cmp
     if (trame_cmp("$GPGGA suite chaine","GPGGA")!=1){
         printf ("Erreur Test unitaire trame_cmp.\n");
         exit(-1);
@@ -171,6 +264,7 @@
         printf ("Erreur Test unitaire decode_int\n");
         exit(-1);
     }
+    //Tests unitaires de conversion
     if (decode_nombre("12",2)!=12)
     {
         printf ("Erreur Test unitaire decode_nombre\n");
@@ -191,6 +285,42 @@
         printf ("Erreur Test unitaire decode_nombre\n");
         exit(-1);
     }
+    //Test unitaires de decode_trame
+    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);
+    }
+    //Tests unitaires de distance a la plus proche zone
+    //Tests unitaires de conversion Latitude et Longitude
+    if (convlat("0045.0000")-(float) 0.75>0.0001)
+    {
+        printf("Erreur Test unitaire convlat\n");
+        exit(-1);
+    }
+    if (convlog("00306.6036")- (float)3.11006>0.0001)
+    {
+        printf("Erreur Test unitaire convlog.\n");
+        exit(-1);
+    }
+    if (convlog("00008.0015")-(float)0.133358>0.0001)
+    {
+        printf("Erreur Test unitaire convlog\n");
+        exit(-1);
+    }
+
 
 }
 
Index: branch/lashermes/sp4a3/sp4a3_kalman.c
===================================================================
--- branch/lashermes/sp4a3/sp4a3_kalman.c	(revision 281)
+++ branch/lashermes/sp4a3/sp4a3_kalman.c	(revision 282)
@@ -7,12 +7,34 @@
 #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])
+{
+    if(na==nb && ma==mb )
+    {
+        int i,j;
+        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]){
-
+void Inverse_Mat_22(int n,int m,double A[n][m],double B[n][m])
+{
+    if (m==2 && n==2)
+    {
+        double det=A[0][0]*A[1][1]-A[0][1]*A[1][0];
+        if(det!=0)
+        {
+            B[0][0]= (double) A[1][1]/det;
+            B[0][1]= (double) -A[0][1]/det;
+            B[1][0]= (double) -A[1][0]/det;
+            B[1][1]= (double) A[0][0]/det;
+        }
+    }
 }
 
 void Transpose_Mat(int n,int m,double A[n][m],double R[m][n]){
@@ -22,12 +44,33 @@
 			R[j][i]=A[i][j];
 }
 
-void Sub_Mat_Mat(int na,int ma,double A[na][ma],int nb,int mb,double b[nb][mb], double out[na][ma]){
+void Sub_Mat_Mat(int na,int ma,double A[na][ma],int nb,int mb,double b[nb][mb], double out[na][ma]){
+    if(na==nb && ma==mb ){
+        int i,j;
+        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,j,k;
+        for(i=0;i<na;i++)
+        {
+            for (j=0;j<mb;j++)
+            {
+                out[i][j]=0;
+                for(k=0;k<ma;k++)
+                {
+                    out[i][j]+=A[i][k]*B[k][j];
+                }
+            }
+        }
+    }
 }
 
 
@@ -137,8 +180,14 @@
 						 {0, 0, 1, 0},
 						 {0, 0, 0, 1}};
 	double FT[4][4];
-	Transpose_Mat(4,4,F,FT);
+	Transpose_Mat(4,4,F,FT);
+
+	double C[2][2];
+
+	double CT[2][2];
+	Transpose_Mat(2,2,C,CT);
 
+
 	while(fscanf(fichier, "%lf %lf %lf", &t, &x, &y)>0){
 		printf("-------------%04d--------------\n",cpt);
 
@@ -154,25 +203,65 @@
 		{
 			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
+			// Kalman
 
-			// X = F*X
-			Plot_Mat(X," X(k+1|k) = ");
+			// X = F*X
+			double A[4][1];
+			Mul_Mat_Mat(4,4,F,4,1,X,A);
+			Plot_Mat(A," X(k+1|k) = ");
 
-			//P = F*P*F'+Q;
-			Plot_Mat(P,"P(k+1|k) = F.P(k|k).FT + Q = ");
+			//P = F*P*F'+Q;
+			double B[4][4];
+			double D[4][4];
+			Mul_Mat_Mat(4,4,F,4,4,P,B);
+			Mul_Mat_Mat(4,4,B,4,4,FT,D);
+			Add_Mat_Mat(4,4,D,4,4,Q,P);
+			Plot_Mat(P,"P(k+1|k) = F.P(k|k).FT + Q = ");
 
-			// K = P*H' / ( H*P*H' + R);
+
+			// K = P*H' / ( H*P*H' + R);
+			double L[4][2];
+			double C[2][2];
+			double E[2][4];
+			double G[2][2];
+
+			Mul_Mat_Mat(4,4,P,4,2,HT,L);
+			Mul_Mat_Mat(2,4,H,4,4,P,E);
+			Mul_Mat_Mat(2,4,E,4,2,HT,G);
+			Add_Mat_Mat(2,2,G,2,2,R,C);
+			Add_Mat_Mat(4,4,L,2,2,CT,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 = ");
+			//Plot_Mat(Delta,"DELTA = Obs - H.X(k+1|k)");
+			double N[2][1];
+			double O[4][1];
+			double posi[2][1]={x,y};
+			double I[2][1];
+			double XC[4][1];
+
+			X[4][1]=XC[4][1];
+
+			Mul_Mat_Mat(2,4,H,4,1,X,I);
+			Sub_Mat_Mat(2,1,I,2,1,posi,N);
+			Mul_Mat_Mat(4,2,K,2,1,N,O);
+			Add_Mat_Mat(4,1,X,4,1,O,XC);
+            Plot_Mat(XC," 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) = ");
+			// P = P - K*H*P;
+			double M[4][4];
+			double J[4][4];
+			double PC[4][4];
+
+			P[4][4]=PC[4][4];
+
+			Mul_Mat_Mat(4,2,K,2,4,H,J);
+			Mul_Mat_Mat(4,4,J,4,4,P,M);
+			Sub_Mat_Mat(4,4,M,4,4,P,PC);
+            Plot_Mat(PC," P(k+1|k+1) = P(k+1|k) - K.H.P(k+1|k) = ");
 
 			/// La matrice X doit contenir la position filtrée ///
 		}
