Index: branch/SENE/sp4a12/main.c
===================================================================
--- branch/SENE/sp4a12/main.c	(revision 920)
+++ branch/SENE/sp4a12/main.c	(revision 921)
@@ -41,7 +41,7 @@
     }
     return a;
 
-}
+}/*
 
 //fonction decode_nombre
 int decode_nombre(char *ch,int n)
@@ -53,7 +53,7 @@
         r=r+tab[i]*pow(10,n-i-1);
     }
     return r;
-    }
+    }*/
 
 
 
@@ -100,16 +100,16 @@
 
 void test_decode_int(void)
 {
-    char i;
-    for(i=0;i<10;i++)
-    {
-        if(decode_int('i')==1)
-            printf("Valeur décimal égale au caractere");
-    }
+   char caract='5';
+   int valeur;
+   valeur = decode_int(caract);
+   printf("La valeur en decimale est : %d\n",valeur);
 
+
+
 }
 
-void test_decode_nombre(void)
+/*void test_decode_nombre(void)
 {
     char *c;
     c="7541";
@@ -119,7 +119,7 @@
         printf("Test validé");
         exit(-1);
     }
-}
+}*/
 
 // Ne pas modifier cette fonction
 int main(int argc,char ** argv)
Index: branch/SENE/sp4c12/Ely_SENE_sp4c12/sp4c/sp4c/sp4c.c
===================================================================
--- branch/SENE/sp4c12/Ely_SENE_sp4c12/sp4c/sp4c/sp4c.c	(revision 920)
+++ branch/SENE/sp4c12/Ely_SENE_sp4c12/sp4c/sp4c/sp4c.c	(revision 921)
@@ -13,74 +13,840 @@
 #include"sfr32c87.h"
 #include <stdlib.h>
 void uart0_tx(char c){
-        
+
         te_u0c1 = 1;
         ti_u0c1 = 0;
         while(ti_u0c1 == 1){
-                u0tb = c;        
-        }        
+                u0tb = c;
+        }
 }
 void main(void)
 {
-        
+
         char c = 'Y';
-        
+
         //U0MR
         smd0_u0mr = 1; //Uart en mode 8bit
         smd1_u0mr = 0;
-         smd2_u0mr = 1; 
+         smd2_u0mr = 1;
         ckdir_u0mr = 0; //horloge interne
         stps_u0mr = 0; //1 bit de stop
         pry_u0mr  = 0; //parit? impair
         prye_u0mr = 0; //parit? d?sactiv?
         iopol_u0mr = 0; // TX RX non inv?rs?
-        
+
         //U0SMR
         u0smr = 0x00;
         u0smr2 = 0x00;
         u0smr3 = 0x00;
         u0smr4 = 0x00;
-        
+
         //U0BRG
-        u0brg = 0x20; // baud rate 
-        
+        u0brg = 0x20; // baud rate
+
         //U0C0
     clk0_u0c0 =1; // selection f8
-           clk1_u0c0 =0;  
-         crs_u0c0 =0; 
-           txept_u0c0 =0; 
-    crd_u0c0 =1; 
-    nch_u0c0 =0;  
-    ckpol_u0c0 =0; 
-    uform_u0c0 =0; 
-        
+           clk1_u0c0 =0;
+         crs_u0c0 =0;
+           txept_u0c0 =0;
+    crd_u0c0 =1;
+    nch_u0c0 =0;
+    ckpol_u0c0 =0;
+    uform_u0c0 =0;
+
         //U0C1
-        te_u0c1 = 1; 
+        te_u0c1 = 1;
         ti_u0c1 = 0;
-        re_u0c1 = 1; 
-        ri_u0c1 = 0; 
-        u0irs_u0c1 = 0; 
-        u0rrm_u0c1 = 0; 
+        re_u0c1 = 1;
+        ri_u0c1 = 0;
+        u0irs_u0c1 = 0;
+        u0rrm_u0c1 = 0;
         u0lch_u0c1 = 0;
-        sclkstpb_u0c1 = 0; 
+        sclkstpb_u0c1 = 0;
         u0ere_u0c1 = 0;
-        
+
         pd6_0 = 0;
         pd6_1 = 0;
         pd6_2 = 0;
         pd6_3 = 1;
-        
-        
+
+
         ps0_0 = 0;
         ps0_1 = 0;
         ps0_2 = 0;
         ps0_3 = 1;
-        
+
         while(1){
                 uart0_tx(c);
                 c=c+1;
                 if(c > 90){
-                        c = 'A';        
+                        c = 'A';
                 }
         }
-}
\ No newline at end of file
+}
+
+ALPHABET
+#include"sfr32c87.h"
+#include <stdlib.h>
+void uart0_tx(char c){
+
+        te_u0c1 = 1;
+        ti_u0c1 = 0;
+        while(ti_u0c1 == 1){
+                u0tb = c;
+        }
+}
+void main(void)
+{
+
+        char c = 'Y';
+
+        //U0MR
+        smd0_u0mr = 1; //Uart en mode 8bit
+        smd1_u0mr = 0;
+         smd2_u0mr = 1;
+        ckdir_u0mr = 0; //horloge interne
+        stps_u0mr = 0; //1 bit de stop
+        pry_u0mr  = 0; //parit? impair
+        prye_u0mr = 0; //parit? d?sactiv?
+        iopol_u0mr = 0; // TX RX non inv?rs?
+
+        //U0SMR
+        u0smr = 0x00;
+        u0smr2 = 0x00;
+        u0smr3 = 0x00;
+        u0smr4 = 0x00;
+
+        //U0BRG
+        u0brg = 0x20; // baud rate
+
+        //U0C0
+    clk0_u0c0 =1; // selection f8
+           clk1_u0c0 =0;
+         crs_u0c0 =0;
+           txept_u0c0 =0;
+    crd_u0c0 =1;
+    nch_u0c0 =0;
+    ckpol_u0c0 =0;
+    uform_u0c0 =0;
+
+        //U0C1
+        te_u0c1 = 1;
+        ti_u0c1 = 0;
+        re_u0c1 = 1;
+        ri_u0c1 = 0;
+        u0irs_u0c1 = 0;
+        u0rrm_u0c1 = 0;
+        u0lch_u0c1 = 0;
+        sclkstpb_u0c1 = 0;
+        u0ere_u0c1 = 0;
+
+        pd6_0 = 0;
+        pd6_1 = 0;
+        pd6_2 = 0;
+        pd6_3 = 1;
+
+
+        ps0_0 = 0;
+        ps0_1 = 0;
+        ps0_2 = 0;
+        ps0_3 = 1;
+
+        while(1){
+                uart0_tx(c);
+                c=c+1;
+                if(c > 90){
+                        c = 'A';
+                }
+        }
+}
+
+
+CLAVIER
+clavier
+#include "sfr32c87.h"
+/* init_keyboard(void);
+void tpo_50ms(void);
+
+unsigned char touche = 0;
+
+void main(void){
+        init_keyboard();
+
+while(1){
+        if ( (p10 & 0xf0)!= 0xf0 ){
+                touche = p10;
+                tpo_50ms();
+                }
+        }
+}
+void init_keyboard(void){
+          pd10 = 0x0F;
+          p10=0;
+          pu31 = 1 ;
+}
+void tpo_50ms(void){
+        tcspr = 0x8A;
+        ta0mr = 0x82;
+        ta0 = 50000;
+        ta0s = 1;
+        ta0os = 1;
+        ta0ic = 0x00;
+while(ir_ta0ic != 1 );
+ta0s = 0;
+}*/
+char t;
+char touche;
+void main(void){
+        pd10=0x0F;
+        pu31=1;
+        p10=0x00;
+        while(1){
+        t=p10;
+        if(t!=0xF0){
+                p10=0xFe;
+                t=p10;
+        if(t==0xde){
+                touche = '2';
+                        }
+        if(t==0xbe){
+                touche = '1';
+                        }
+        if(t==0xfe){
+                touche = '7';
+                        }
+        if(t==0x7e){
+                touche = '3';
+                        }
+                p10=0xFd;
+                t=p10;
+                        if(t==0xbd){
+                        touche = '4';
+                        }
+                        if(t==0xdd){
+                touche = '5';
+                        }
+                        if(t==0x7d){
+                touche = '6';
+                        }
+                p10=0xFb;
+                t=p10;
+                        if(t==0xdb){
+                touche = '8';
+                        }
+                        if(t==0x7b){
+                touche = '9';
+                        }
+                        if(t==0xeb){
+                touche = '7';
+                        }
+                p10=0xF7;
+                t=p10;
+                        if(t==0xd7){
+                touche = '0';
+                        }
+                        if(t==0xe7){
+                touche = '*';
+                        }
+                        if(t==0x77){
+                touche = '#';
+                        }
+
+
+        }
+        }
+}
+
+ENVOIE CHAINE DE CARACT
+envoie chaine de caractere
+#include"sfr32c87.h"
+#include <stdlib.h>
+void uart0_tx(char c){
+
+        te_u0c1 = 1;
+        ti_u0c1 = 0;
+        while(ti_u0c1 == 1){
+                u0tb = c;
+        }
+}
+void main(void)
+{
+
+        char c = 'Y';
+
+        //U0MR
+        smd0_u0mr = 1; //Uart en mode 8bit
+        smd1_u0mr = 0;
+         smd2_u0mr = 1;
+        ckdir_u0mr = 0; //horloge interne
+        stps_u0mr = 0; //1 bit de stop
+        pry_u0mr  = 0; //parit? impair
+        prye_u0mr = 0; //parit? d?sactiv?
+        iopol_u0mr = 0; // TX RX non inv?rs?
+
+        //U0SMR
+        u0smr = 0x00;
+        u0smr2 = 0x00;
+        u0smr3 = 0x00;
+        u0smr4 = 0x00;
+
+        //U0BRG
+        u0brg = 0x20; // baud rate
+
+        //U0C0
+    clk0_u0c0 =1; // selection f8
+           clk1_u0c0 =0;
+         crs_u0c0 =0;
+           txept_u0c0 =0;
+    crd_u0c0 =1;
+    nch_u0c0 =0;
+    ckpol_u0c0 =0;
+    uform_u0c0 =0;
+
+        //U0C1
+        te_u0c1 = 1;
+        ti_u0c1 = 0;
+        re_u0c1 = 1;
+        ri_u0c1 = 0;
+        u0irs_u0c1 = 0;
+        u0rrm_u0c1 = 0;
+        u0lch_u0c1 = 0;
+        sclkstpb_u0c1 = 0;
+        u0ere_u0c1 = 0;
+
+        pd6_0 = 0;
+        pd6_1 = 0;
+        pd6_2 = 0;
+        pd6_3 = 1;
+
+
+        ps0_0 = 0;
+        ps0_1 = 0;
+        ps0_2 = 0;
+        ps0_3 = 1;
+
+        while(1){
+                uart0_tx(c);
+                c=c+1;
+                if(c > 90){
+                        c = 'A';
+                }
+        }
+}
+
+IRECTRAME
+irectrame
+#include "sfr32c87.h"
+void uart0_init(void); // Initialisation du port s?rie
+void uart0_tx(char c); //Envoie d'un caract?re
+char uart0_rx(void); //Fonction d'attente et lecture d'un caract?re
+int rectramev1(char * Buffer);
+int rectramev2(char * Buffer);
+void irectramev1(char * Buffer);
+int hex2int(char *c); //Passage hexa en int
+/*Variable Globale*/
+int trame_ok;
+int Etat = 0; //0->synchronisation, 1->reception, 2->checksum, 3->validation
+void main(void)
+{
+        char Buffer[80];
+
+        uart0_init();
+        while(1){
+                uart0_tx(rectramev1(Buffer)+48);
+                if (trame_ok == 1){
+                        uart0_tx('O');
+                        uart0_tx('K');
+                }
+                else{
+                        uart0_tx('N');
+                        uart0_tx('O');
+                        uart0_tx('N');
+                }
+        }
+}
+
+
+void uart0_init(void){
+        u0mr = 0x05;
+        u0brg = 32;
+        u0c0 = 0x11;
+        u0c1 = 0x05;
+        pd6_0 = 0;
+        pd6_1 = 0;
+        pd6_2 = 0;
+        pd6_3 = 1; //On affecte seulement les bits que l'on a besoin pour le bon fonctionnement
+        ps0_3 = 1;
+}
+void uart0_tx(char c){
+        while (ti_u0c1 != 1);
+        u0tb = c;
+}
+char uart0_rx(void){
+        while (ri_u0c1 != 1);
+        return u0rb;
+}
+int rectramev1(char * Buffer){
+        int i;
+        char checksum;
+        char carac_recu;
+        char checksum_trame[3];
+
+        while (uart0_rx() != '$');
+        i = 0;
+        checksum = 0; //'0'
+        while ((carac_recu = uart0_rx()) != '*'){
+
+                Buffer[i++] = carac_recu;
+                checksum = checksum ^ carac_recu; // ^ est le ou exclusif
+        }
+
+        checksum_trame[0] = uart0_rx();
+        checksum_trame[1] = uart0_rx();
+
+        if (checksum == hex2int(checksum_trame)){
+                trame_ok = 1;
+        }
+        else{
+                trame_ok = 0;
+        }
+        return i;
+}
+/*int rectramev2(char * Buffer){
+        int i;
+        char c;
+        while(1){
+                c = u0rb;
+                if (c == '&'){
+
+                }
+        }
+
+        return i;
+}*/
+void irectramev1(char * Buffer){
+        int i;
+        char c;
+        char checksum_trame;
+        while(1){
+
+                c = uart0_rx();
+                switch(Etat){
+
+                        case 0 : //Synchro
+
+                                if (c == '$'){
+
+                                        i = 0;
+                                        checksum_trame = 0;
+                                        Etat = 1;
+                                }
+
+                        case 1 : //Reception
+
+                                if (c != '*'){
+
+                                        Buffer[i++] = c;
+                                        checksum_trame ^= c;
+
+                                }
+                                else{
+
+                                        Etat = 2;
+                                }
+
+                        case 2 : //checksum
+
+
+
+
+
+                }
+
+        }
+}
+int hex2int(char *c)
+{
+        int i=0;
+        for(i=0; i<2; i++)
+        {
+                if(c[i]>='0' && c[i]<='9')
+                {
+                        c[i] = c[i] - '0';
+                }
+                if(c[i]>='A' && c[i]<='F')
+                {
+                        c[i] = c[i] - 'A' + 10;
+                }
+        }
+        return(c[1]+16*c[0]);
+}
+/*
+typedef enum{synchronisation , reception , checksum , validation}etat_machine;
+etat_machine etat=synchronisation;
+
+
+int RecTrame(char*Buffer)
+{
+        //declaration des variables
+        int nombre_caractere_apres_etoile=0;
+        int k=0; //pour le test
+        int nombre_cararactere=0;
+        char caractere_recu;
+        char checksum_trame=0;
+        char checksum_fin_trame[3];
+        char checksum_calculer=0;
+
+        while(1)
+        {
+                //caractere_recu = uart0_rx();
+                caractere_recu = trame1[k];
+                k++;
+
+                switch(etat)
+                {
+                        case synchronisation :
+                                        if(caractere_recu == '$')
+                                        {
+                                                nombre_cararactere = 0;
+                                                etat=reception;
+                                                checksum_trame = 0;
+                                        }
+                                        break;
+
+                        case reception :
+                                if(caractere_recu != '*')
+                                {
+                                    //placement du caractere recu de la trame dans le Buffer
+                                        Buffer[nombre_cararactere] = caractere_recu;
+                                        nombre_cararactere++;
+                                //verification du checksum ? chaque caractere recu
+                                        checksum_trame ^= caractere_recu;
+                                        nombre_caractere_apres_etoile = 0;
+                                }
+                                else
+                                {
+                                    //passage ? l'etat cheksum quand la trame est memoris?e
+                                        etat = checksum;
+                                }
+                                break;
+
+                        case checksum :
+                                            //r?cup?ration du checksum de la trame (2derniers caract?res)
+                        checksum_fin_trame[nombre_caractere_apres_etoile] = caractere_recu;
+
+                                                    //attention, on m?morise deux caract?res
+                                                donc, premier caract?re -> nombre_caractere_apres_etoile=0
+                                                           deuxi?me caract?re -> nombre_caractere_apres_etoile=1
+                                                soit deux caract?res m?moris?s apr?s la virgule
+
+                                if(nombre_caractere_apres_etoile == 2)
+                                {
+                                            etat = validation;
+                                        checksum_calculer = hex_int(checksum_fin_trame);
+                                }
+
+                                nombre_caractere_apres_etoile++;
+                                break;
+
+                        case validation :
+                                            //comparaison du checksum_trame et checksum_calculer
+                                                            si l'?galit? est v?rifi?e alors on retourne le nombre
+                                            de caract?re de la trame, sinon on retourne la valeur 0
+
+                                if(checksum_trame == checksum_calculer)
+                                {
+                                        etat = synchronisation;
+                                        return nombre_cararactere;
+                                }
+                                else
+                                {
+                                        etat = synchronisation;
+                                        return 0;
+                                }
+                                break;
+                }
+        }
+}
+*/
+
+KALMAN
+Kalman
+//  Pour compiler : gcc sp4a3_kalman.c  -lm
+#include <stdlib.h>
+#include <stdio.h>
+#include <math.h>
+#include "sp4a3_kalman_extra.h"
+void init(int n, int m, double R22 [n][m])
+{
+short i=0,j=0;
+for (i=0;i<n;i++)
+    {
+        for (j=0;j<m;j++)
+        {
+            R22[i][j]=0;
+        }
+    }
+}
+void Add_Mat_Mat(int na,int ma,double A[na][ma],int nb,int mb,double B[nb][mb], double OUT[na][ma])
+{
+short i=0,j=0;
+    init (na,ma,OUT); // initialisation Resultat
+    for (i=0;i<na;i++)
+    {
+        for (j=0;j<ma;j++)
+        {
+            OUT[i][j]=A[i][j]+B[i][j];
+        }
+    }
+}
+void Inverse_Mat_22(int n,int m,double A[n][m],double OUT[n][m])
+{
+short i=0,j=0;
+    init (n,m,OUT); // initialisation Resultat
+    if ((A[0][0]*A[1][1]-A[0][1]*A[1][0])==0)
+    {
+        printf ("Erreur d'inversion : determinant de la matrice nul");
+        exit (-1);
+    }
+    for (i=0;i<n;i++)
+    {
+        for (j=0;j<m;j++)
+        {
+            if (i!=j)
+            {
+                OUT[i][j]= -(1/(A[0][0]*A[1][1]-A[0][1]*A[1][0]) * A[i][j]);
+                OUT[i][i]= (1/(A[0][0]*A[1][1]-A[0][1]*A[1][0]) * A[j][j]);
+            }
+        }
+    }
+}
+void Transpose_Mat(int n,int m,double A[n][m],double R[m][n])
+{
+short i=0,j=0;
+    init (m,n,R); // initialisation Resultat
+    for (i=0;i<m;i++)
+    {
+        for (j=0;j<n;j++)
+        {
+            R[i][j]=A[j][i];
+        }
+    }
+}
+void Sub_Mat_Mat(int na,int ma,double A[na][ma],int nb,int mb,double B[nb][mb], double OUT[na][ma])
+{
+short i=0,j=0;
+    init (na,ma,OUT); // initialisation Resultat
+    for (i=0;i<na;i++)
+    {
+        for (j=0;j<ma;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])
+{
+short i=0,j=0,k=0;
+    init (na,mb,OUT); // initialisation Resultat
+    if (ma!=nb)
+    {
+        printf ("Erreur dimension matrice multiplication.");
+        exit (-1);
+    }
+    for (i=0;i<na;i++)
+    {
+        for (j=0;j<mb;j++)
+        {
+            for (k=0;k<ma;k++)
+            {
+                OUT[i][j]= OUT[i][j] + (A[i][k]*B[k][j]);
+            }
+        }
+    }
+}
+void tests_unitaires(void){
+        //Matrices d'entrée
+        double T21a[2][1]={{7},{-5}};
+        double T21b[2][1]={{-3},{46}};
+        double T22a[2][2]={{12,78},{-5,13}};
+        double T22b[2][2]={{-25,36},{7,42}};
+        double T24[2][4]={{7,-71,-12,3},{41,123,-5,10}};
+        double T41a[4][1]={{45},{-123},{-78},{-410}};
+        double T41b[4][1]={{-10},{45},{27},{-9}};
+        double T42a[4][2]={{-73,45},{10,12},{-41,-35},{8,-23}};
+        double T44a[4][4]={{1,2,7,4},{6,5,7,8},{9,8,7,6},{5,4,3,2}};
+        double T44b[4][4]={{12,13,14,15},{21,22,23,40},{78,45,12,3},{54,10,12,47}};
+        //Matrices résultat
+        double R21[2][1],R22[2][2],R24[2][4],R41[4][1],R42[4][2],R44[4][4];
+        //Matrices de validation
+        double RST21[2][1]={{10},{-51}};
+        double RInvT22[2][2]={{0.02380952380952381,-0.1428571428571428},{0.009157509157509158,0.02197802197802198}};
+        double RAT22[2][2]={{-13,114},{2,55}};
+        double RTT24[4][2]={{7,41},{-71,123},{-12,-5},{3,10}};
+        double RMT24T41[2][1]={{8754},{-16994}};
+        double RMT24T42[2][2]={{-705,-186},{-1478,3266}};
+        double RMT24T44[2][4]={{-512,-425,-523,-606},{784,697,1143,1138}};
+        double RAT41[4][1]={{35},{-78},{-51},{-419}};
+        double RMT42T21[4][1]={{-736},{10},{-112},{171}};
+        double RMT42T22[4][2]={{-1101,-5109},{60,936},{-317,-3653},{211,325}};
+        double RMT42T24[4][4]={{1334,10718,651,231},{562,766,-180,150},{-1722,-1394,667,-473},{-887,-3397,19,-206}};
+        double RTT44[4][4]={{1,6,9,5},{2,5,8,4},{7,7,7,3},{4,8,6,2}};
+        double RMT44T41[4][1]={{-2387},{-4171},{-3585},{-1321}};
+        double RMT44T42[4][2]={{-308,-268},{-611,-99},{-816,118},{-432,122}};
+        double RAT44[4][4]={{13,15,21,19},{27,27,30,48},{87,53,19,9},{59,14,15,49}};
+        double RST44[4][4]={{-11,-11,-7,-11},{-15,-17,-16,-32},{-69,-37,-5,3},{-49,-6,-9,-45}};
+        double RMT44T44[4][4]={{816,412,192,304},{1155,583,379,687},{1146,668,466,758},{486,308,222,338}};
+        printf("Execution des tests unitaires.\n");
+        Transpose_Mat(2,4,T24,R42);                if (!Equal_Mat_Mat(RTT24,R42))                 error("Erreur calcul Transposition 2x4");
+        Transpose_Mat(4,4,T44a,R44);        if (!Equal_Mat_Mat(RTT44,R44))                 error("Erreur calcul Transposition 4x4");
+        Inverse_Mat_22(2,2,T22a,R22);        if (!Equal_Mat_Mat(RInvT22,R22))         error("Erreur calcul Inversion 2x2");
+        Add_Mat_Mat(2,2,T22a,2,2,T22b,R22);        if (!Equal_Mat_Mat(RAT22,R22))                 error("Erreur calcul Addition 2x2");
+        Add_Mat_Mat(4,4,T44a,4,4,T44b,R44);        if (!Equal_Mat_Mat(RAT44,R44))                 error("Erreur calcul Addition 4x4");
+        Add_Mat_Mat(4,1,T41a,4,1,T41b,R41);        if (!Equal_Mat_Mat(RAT41,R41))                 error("Erreur calcul Addition 4x1");
+        Sub_Mat_Mat(2,1,T21a,2,1,T21b,R21);        if (!Equal_Mat_Mat(RST21,R21))                 error("Erreur calcul Soustraction 2x1");
+        Sub_Mat_Mat(4,4,T44a,4,4,T44b,R44);        if (!Equal_Mat_Mat(RST44,R44))                 error("Erreur calcul Soustraction 4x4");
+        Mul_Mat_Mat(4,4,T44a,4,4,T44b,R44);        if (!Equal_Mat_Mat(RMT44T44,R44))         error("Erreur calcul Multiplication 4x4 4x4");
+        Mul_Mat_Mat(4,4,T44a,4,1,T41a,R41);        if (!Equal_Mat_Mat(RMT44T41,R41))         error("Erreur calcul Multiplication 4x4 4x1");
+        Mul_Mat_Mat(4,4,T44a,4,2,T42a,R42);        if (!Equal_Mat_Mat(RMT44T42,R42))         error("Erreur calcul Multiplication 4x4 4x2");
+        Mul_Mat_Mat(4,2,T42a,2,1,T21a,R41);        if (!Equal_Mat_Mat(RMT42T21,R41))         error("Erreur calcul Multiplication 4x2 2x1");
+        Mul_Mat_Mat(4,2,T42a,2,2,T22a,R42);        if (!Equal_Mat_Mat(RMT42T22,R42))         error("Erreur calcul Multiplication 4x2 2x2");
+        Mul_Mat_Mat(4,2,T42a,2,4,T24,R44);        if (!Equal_Mat_Mat(RMT42T24,R44))         error("Erreur calcul Multiplication 4x2 2x4");
+        Mul_Mat_Mat(2,4,T24,4,1,T41a,R21);        if (!Equal_Mat_Mat(RMT24T41,R21))         error("Erreur calcul Multiplication 2x4 4x1");
+        Mul_Mat_Mat(2,4,T24,4,2,T42a,R22);        if (!Equal_Mat_Mat(RMT24T42,R22))         error("Erreur calcul Multiplication 2x4 4x2");
+        Mul_Mat_Mat(2,4,T24,4,4,T44a,R24);        if (!Equal_Mat_Mat(RMT24T44,R24))         error("Erreur calcul Multiplication 2x4 4x4");
+        printf("Test unitaires OK.\n");
+    getchar();
+}
+int main(int argc,char **argv){
+        tests_unitaires();
+        FILE* fichier = fopen("pos_t_x_y.dat","r");
+        if (fichier == NULL)
+        error("Impossible d'ouvrir le fichier GPGGA_data.dat");
+    FILE * Fout = Fout = fopen("output.dat","w");
+        if (fichier == NULL)
+        error("Impossible d'ouvrir le fichier output.dat");
+        printf("Kalman\n");
+        double t = 0;
+        double t0,x0,y0;
+        double x,y;
+        double oldx,oldy;
+        double dx=0,dy=0,dt=0.1;
+        int cpt = 0;
+        // kalman param
+        double sigma_etat = 10.0;
+        double sigma_observation = 2.0;
+        double X[4][1] = {{0},{0},{0},{0}};
+        double P[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 Q[4][4] =        {{0, 0, 0, 0},
+                                                 {0, 0, 0, 0},
+                                                 {0, 0, 0.1, 0},
+                                                 {0, 0, 0, 0.1}};
+        double R[2][2] =         {{sigma_observation*sigma_observation, 0},
+                                                 {0 , sigma_observation*sigma_observation}};
+        double K[4][2];
+        double H[2][4] =         {{1, 0, 0, 0},
+                                                 {0, 1, 0, 0}};
+        double HT[4][2];
+        Transpose_Mat(2,4,H,HT);
+        double F[4][4] =        {{1, 0, dt, 0},
+                                                 {0, 1, 0, dt},
+                                                 {0, 0, 1, 0},
+                                                 {0, 0, 0, 1}};
+        double FT[4][4];
+        Transpose_Mat(4,4,F,FT);
+        while(fscanf(fichier, "%lf %lf %lf", &t, &x, &y)>0){
+                printf("-------------%04d--------------\n",cpt);
+                if (cpt ==0)
+                {
+                        t0=t;x0=x;y0=y;
+                        x=x-x0;y=y-y0;
+                        Plot_Mat(F,"F = ");
+                        Plot_Mat(H,"H = ");
+                        Plot_Mat(R,"R = ");
+                }
+                else
+                {
+                        t -= t0;x -= x0;y -= y0;
+                        debug=1; ///Mettre à 1 pour afficher les matrices.
+                        ///Ajouter votre code ci-dessous///
+                        // Kalman
+        //Matrices temporaires
+            double Temp44[4][4]={{0,0,0,0},{0,0,0,0},{0,0,0,0},{0,0,0,0}};
+            double Temp24[2][4]={{0,0,0,0},{0,0,0,0}};
+            double Temp42[4][2]={{0,0},{0,0},{0,0},{0,0}};
+            double Temp22[2][2]={{0,0},{0,0}};
+            double Temp21[2][1]={{0},{0}};
+            double Temp41[4][1]={{0},{0},{0},{0}};
+            double Res0[4][1];
+            double Res1[4][4];
+            double Res2[2][2];
+            double Res3[4][1];
+            double Res4[4][4];
+        //Prediction
+            Plot_Mat(X," X(k|k) = ");
+            //X=F*X
+            Mul_Mat_Mat(4,4,F,4,1,X,Res0);
+            Add_Mat_Mat(4,1,Res0,4,1,Temp41,X);   // Temp41 est initialisée à 0 donc grace à ca on affecte à X la matrice Res0
+            Plot_Mat(X,"X(k+1|k) = F*X(k|k) = ");
+            Plot_Mat(P,"P(k|k) = ");
+            //P=F*P*F'+Q
+            Mul_Mat_Mat(4,4,F,4,4,P,Temp44);
+            Mul_Mat_Mat(4,4,Temp44,4,4,FT,Res1);
+            Add_Mat_Mat(4,4,Res1,4,4,Q,P);
+            Plot_Mat(P,"P(k+1|k) = F.P(k|k).FT + Q = ");
+        //Gain
+            //K=P*H'/(H*P*H'+R)
+            Mul_Mat_Mat(4,4,P,4,2,HT,Temp42);
+            Plot_Mat(Temp42,"P(k+1|k).HT = ");
+            Mul_Mat_Mat(2,4,H,4,2,Temp42,Temp22);
+            Add_Mat_Mat(2,2,Temp22,2,2,R,Res2);
+            Plot_Mat(Res2,"H.P(k+1|k).HT + R = ");
+            Inverse_Mat_22(2,2,Res2,Temp22);
+            Plot_Mat(Temp22,"INV(H.P(k+1|k).HT + R) = ");
+            Mul_Mat_Mat(4,2,HT,2,2,Temp22,Temp42);
+            Mul_Mat_Mat(4,4,P,4,2,Temp42,K);
+            Plot_Mat(K,"K = ");
+        //MaJ
+            double VECT [2][1]={{x},{y}};
+            Plot_Mat(VECT,"obs = ");
+            //DELTA = Obs - H.X(k+1|k)
+            double Delta [2][1];
+            Mul_Mat_Mat(2,4,H,4,1,X,Temp21);
+            Sub_Mat_Mat(2,1,VECT,2,1,Temp21,Delta);
+            Plot_Mat(Delta,"DELTA = Obs - H.X(k+1|k) = ");
+            //X = X + K*([xb(i);yb(i)]-H*X);
+            Mul_Mat_Mat(4,2,K,2,1,Delta,Temp41);
+            Add_Mat_Mat(4,1,X,4,1,Temp41,Res3);
+            init(4,1,Temp41);                   //Temp41 = 0
+            Add_Mat_Mat(4,1,Temp41,4,1,Res3,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 = ");
+            init(2,4,Temp24);
+            // P = P - K*H*P;
+            Mul_Mat_Mat(2,4,H,4,4,P,Temp24);
+            Mul_Mat_Mat(4,2,K,2,4,Temp24,Temp44);
+            Sub_Mat_Mat(4,4,P,4,4,Temp44,Res4);
+            init(4,4,Temp44);   //Temp44 est égale à 0.
+            Add_Mat_Mat(4,4,Res4,4,4,Temp44,P); // P=Res3+0 donc on affecte à P la matrice Res3
+            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 = (x - oldx)/dt;
+                dy = (y - oldy)/dt;
+                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 ++;
+        }
+        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 'vitesse_reelle.dat' u 2 w l, 'output.dat' u 9 w l\";");
+        return 0;
+}
