Index: branch/GUIFO/sp4a12/main.c
===================================================================
--- branch/GUIFO/sp4a12/main.c	(revision 670)
+++ branch/GUIFO/sp4a12/main.c	(revision 671)
@@ -3,11 +3,10 @@
 #include <strings.h>
 #include "trame.h"
 #include "math.h"
-#define k 111120
+#define k 111.120 //Constante a utiliser pour calculer la distance en utilisant la m�thode de Pythagore
 #define N 200
-#define rads 0.01745329252
-#define t 1/6
-#define h 1/60
+#define rads 0.01745329252 //rads=pie/180 puisque le langage C utilise les angles en radians
+
 
 //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",
@@ -18,7 +17,10 @@
                     "$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};
+
 
 typedef struct {
     float latitude;
@@ -25,6 +27,7 @@
     float longitude;
 } Position ;
 
+
 //Comparaison des trames pour savoir si la trame est du bon type
 int trame_cmp(char* trame,char* type)
 {
@@ -46,6 +49,7 @@
    return d;
 
 }
+
 //Conversion d'un caract�re en decimal
 int decode_int(char c)
 {
@@ -60,6 +64,7 @@
     }
     return d;
 }
+
 //Renvoi la valeur decimal des n premiers caract�res entr�e en param�tre
 int decode_nombre(char *c, int n)
 {
@@ -69,12 +74,13 @@
     }
     return valeur;
 }
+
 //Conversion de la latitude et longitude en degree floatant
 float conversion(char *c)
 {
     int i=0;
     float deg_lat,deg_long;
-    float test;
+
     while(c[i]!='\0'){
 
       i++;
@@ -134,6 +140,7 @@
     }
     else{
         d=0;
+        printf("\nNe peut decode cette trame");
 
      }
      printf("\nTrames: %s", trame);
@@ -141,8 +148,28 @@
      printf("\nLongitude: %2.7f", (*p).longitude);
 
 }
-//Test pour d�coder la latitude et la longitude des trames "$GPGGA" en degr� d�cimaux
 
+// calcule la racine carr�e en se basant sur l'algorithme de Heron
+float racine_carree(float nb){
+float a,fa,e;
+	e=1;
+	a=1;
+	while(e>0.0001){
+
+		fa=((nb/a)+a)/2;
+		if(fa>a)
+		{
+			e=fa-a;
+
+		}
+			else
+			e=a-fa;
+		a=fa;
+
+	}
+	return a;
+}
+
 //Fonction qui calcule la distance entre deux positions
 float calcul_distance(Position p1,Position p2){
     float x,a,y,z,distance,b;
@@ -150,7 +177,7 @@
     b=(p1.latitude+p2.latitude)/2;
     x=a*cos(b*rads);
     y=p2.latitude-p1.latitude;
-    z=sqrt((x*x)+(y*y));
+    z=racine_carree((x*x)+(y*y));
     distance=k*z;
     return distance;
 
@@ -157,19 +184,49 @@
 }
 
 //Fonction qui calcule la vitesse du vehicule par seconde
-/*float calcule_vitesse(Position p1, Position p2){
+float calcule_vitesse(Position p1, Position p2){
 
     float distance,vitesse;
-    distance=calcul_distance(Position p1, Position p2);
+    distance=calcul_distance( p1, p2);
     vitesse=3.6*distance;
-    return vitesse;*
+    return vitesse;
 
-}*/
+}
+typedef struct {
+Position rpos;
+float vitmax;
+} Zone ;
+
+Zone zones[] = {
+        {{44.7887762, -3.012}, 50}, /* Descripteur de la premi�re zone */
+        {{44.7891220, -3,013}, 70},
+};
+
+//Calcul de la distance � la zone dangereuse la plus proche
+int distance_a_la_proche_zone(Position p, Zone r[],int nb_zones, float *d){
+    int i;
+    float dist;
+    if(nb_zones!=0){
+        (*d)=calcul_distance(p,r[0].rpos);
+
+        for(i=1;i<nb_zones;i++){
+
+            dist=calcul_distance(p,r[i].rpos);
+
+            if(dist<(*d)){
+                (*d)=dist;
+            }
+        }
+
+    }
+    return i;
+
+}
 //Fonction � modifier !!!!!
 void traitement(char * trame)
 {
     Position p;
-    int d;
+
     static int cpt=0 ;
     cpt++ ;
     if (trame_cmp(trame,"GPGGA")==1)
Index: branch/GUIFO/sp4a3/sp4a3_kalman.c
===================================================================
--- branch/GUIFO/sp4a3/sp4a3_kalman.c	(revision 670)
+++ branch/GUIFO/sp4a3/sp4a3_kalman.c	(revision 671)
@@ -43,17 +43,24 @@
 //Multiplication de matrices
 void Mul_Mat_Mat(int na,int ma,double A[na][ma], int nb,int mb,double B[nb][mb], double out[na][mb]){
     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]);
-         }
-}
+    double C[na][mb];
+    if(ma==nb){
+        for (i=0; i<na; i++){
+            for (j=0; j<mb; j++)
+            {
+                C[i][j]=0;
+                for (k=0; k<nb; k++){
+                    C[i][j] += (A[i][k])*(B[k][j]);
+                }
+                 out[i][j]=C[i][j];
+            }
+        }
+    }
+}
 
 
 
+
 void tests_unitaires(void){
 	//Matrices d'entrée
 	double T21a[2][1]={{7},{-5}};
@@ -111,6 +118,7 @@
 }
 
 int main(int argc,char **argv){
+
 
 	tests_unitaires();
 
@@ -179,43 +187,50 @@
 			debug=1; ///Mettre à 1 pour afficher les matrices.
 			///Ajouter votre code ci-dessous///
 			// Kalman
-
+            Plot_Mat(X," X(k|k) = ");
 			// X = F*X
 			Mul_Mat_Mat(4,4,F,4,1,X,X);
 			Plot_Mat(X," X(k+1|k) = ");
-
+            Plot_Mat(P,"P(k|k) = ");
 			//P = F*P*F'+Q;
-			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);
+            double FP[4][4],FPFT[4][4];
+			Mul_Mat_Mat(4,4,F,4,4,P,FP);
+			Mul_Mat_Mat(4,4,FP,4,4,FT,FPFT);
+			Add_Mat_Mat(4,4,FPFT,4,4,Q,P);
 			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,A2);
-			Mul_Mat_Mat(2,4,H,4,2,A2,B);
-			Add_Mat_Mat(2,2,B,2,2,R,C);
+			double HP[2][4],HPHT[2][2],D[2][2],C[2][2];
+			double PHT[4][2];
+			Mul_Mat_Mat(2,4,H,4,4,P,HP);
+			Mul_Mat_Mat(4,4,P,4,2,HT,PHT);
+			Plot_Mat(PHT,"P(k+1|k).HT = ");
+			Mul_Mat_Mat(2,4,HP,4,2,HT,HPHT);
+			Add_Mat_Mat(2,2,HPHT,2,2,R,C);
+			Plot_Mat(C,"H.P(k+1|k).HT+R = ");
 			Inverse_Mat_22(2,2,C,D) ;
-            Mul_Mat_Mat(4,2,A2,2,2,D,K);
+			Plot_Mat(D,"INV(H.P(k+1|k).HT+R) = ");
+            Mul_Mat_Mat(4,2,PHT,2,2,D,K);
 			Plot_Mat(K,"K = ");
 
 			//X = X + K*([xb(i);yb(i)]-H*X);
+			double delta[2][1],Obs[2][1],Kdelta[4][1],HX[2][1];
 			Obs[0][0]=x;
 			Obs[1][0]=y;
-            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(Obs,"Obs = ");
-            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(Obs,"Obs = ");
+            Mul_Mat_Mat(2,4,H,4,1,X,HX);
+            Sub_Mat_Mat(2,1,Obs,2,1,HX,delta);
+            Plot_Mat(delta,"DELTA = Obs - H.X(k+1|k)");
+            Mul_Mat_Mat(4,2,K,2,1,delta,Kdelta);
+            Add_Mat_Mat(4,1,X,4,1,Kdelta,X);
+            Plot_Mat(X," 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,A3);
-            Mul_Mat_Mat(4,4,A3,4,4,P,A4);
-            Sub_Mat_Mat(4,4,P,4,4,A4,P);
+            double KH[4][4],KHP[4][4];
+            Mul_Mat_Mat(4,2,K,2,4,H,KH);
+            Mul_Mat_Mat(4,4,KH,4,4,P,KHP);
+            Sub_Mat_Mat(4,4,P,4,4,KHP,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 ///
@@ -226,7 +241,8 @@
 		fprintf(Fout,"%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\n",t,x,y,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 = x;
 		oldy = y;
-		cpt ++;
+		cpt ++;
+
 	}
 	fclose(Fout);
 	fclose(fichier);
