essai-gps-fusion.cpp

00001 /*
00002 Test du module GPS
00003 */
00004 
00005 
00006 
00007 
00008 #include "mbed.h"
00009 #include "time.h"
00010 #include "BME280.h"
00011 #include "BNO055.h"
00012 #include "ds3231.h"
00013 #include <Eigen/Dense.h> 
00014 #include "objet.h"
00015 #include <stdio.h>
00016 #include <stdlib.h>
00017 #include <string>
00018 #include <iostream>
00019 #include <fstream>
00020 #include <sstream>
00021 
00022 using namespace std;
00023 using namespace Eigen;
00024 
00025 #define ON  1
00026 #define OFF 0
00027 
00028 // Definition/ Constantes/ Caracteres Alphanumeriques
00029 #define LF                10                                                    // Line Feed \n
00030 #define CR                13                                                    // Carriage Return \r
00031 #define BS               127                                                    // Back Space
00032 
00033 // Definition/ Constantes/  Trigonométriques
00034 #define  PI       3.14159265358979323846                                        // PI 
00035 #define  PI2      1.570796327                                                   // PI/ 2
00036 #define  RD      57.29577951                                                    // 180/ PI conversion radian ---> degre
00037 
00038 // Definition/ Constantes/  Geographiques Terrestres
00039 #define  Rkm   6366.71                                                          // Rayon moyen de la terre exprimé en KMs
00040 #define MAJA   (6378137.0)                                                      // Axe principal de l'ellipsoid de référence
00041 #define FLAT   (1.0/298.2572235630)                                             // Coefficient de planéité de l'ellipsoid de référence
00042 #define ESQR   (FLAT * (2.0-FLAT))                                              // WGS-84/ 1st eccentricity squared 
00043 #define OMES   (1.0 - ESQR)                                                     // WGS-84/ 1 minus eccentricity squared 
00044 #define EFOR   (ESQR * ESQR)                                                    // WGS-84/ Sqr of the 1st eccentricity squared 
00045 #define ASQR   (MAJA * MAJA)                                                    // WGS-84/ semi major axis squared = nlmaja** 
00046 
00047 //Definition/ Constantes
00048 #define TSample         0.01
00049 #define   usbBR    115200
00050 #define   uartBR    38400
00051 #define   gpsBR     57600
00052 #define   i2cBR    400000
00053 #define  gpsWDTM      300
00054 
00055 // Definition/ Constantes/ Dimension MAX Matrice
00056 #define matrixSMAX      4                                                       // Matrice/ Dimension
00057 
00058 #define produit          "GPS-Fusion-Kalman"
00059 
00060 #define NbEch          20
00061 
00062 //Definition/ Variables Globales
00063 DigitalOut   l1(LED1);
00064 DigitalOut   l2(LED2);
00065 DigitalOut   l3(LED3);
00066 DigitalOut   l4(LED4);
00067 DigitalOut   son(p8);
00068 Serial       USB(USBTX, USBRX);                                                 // Liaison UART/ Dispositif TRAME USB
00069 Serial       UART1 (p13,   p14);                                                // Liaison UART/ Dispositif TRAME HF
00070 Serial       UART2 (p28,    p27);                                               // Liaison UART/ Dispositif GPS
00071 I2C          i2c(p9, p10);                                                      // Liaison Bus I2C
00072 //BME280       pression(&i2c);
00073 BNO055       attitude(p9, p10);
00074 GPS_t        gps;
00075 IMU_t        imu;
00076 Ticker       TMainProcess;
00077 string UART1Rx= "";
00078 string UART2Rx= "";
00079 char UART1Tx[100];
00080 char gpsNMEA[100];
00081 string USBRx  = " ";
00082 char   word[25];
00083 bool prn= false;
00084 
00085 string trameID= "$GPRMC";
00086 int pTRAME = 1; 
00087 int l1WDT  = 0;
00088 int l2WDT  = 0;
00089 int l3WDT  = 0;
00090 int l4WDT  = 0;
00091 int gpsWDT = 0;
00092 int imuWDT = 0;
00093 
00094 int cT= 0;
00095 
00096 float  utc= 0;
00097 string valid= "\0";
00098 string id= "";
00099 string val= "\0";
00100 
00101 
00102 int   cnt= 0;
00103 float signal= 0; 
00104 float periode= 0.5;
00105 
00106 
00107 //Definition/ Variables Globales/ Processus Principal/ Variables Matricielles/ Identification Processus
00108 MatrixXd  A(matrixSMAX,matrixSMAX);                                             // Matrice/ Etat Systeme
00109 MatrixXd  B(matrixSMAX,matrixSMAX);                                             // Matrice/ Commande Systeme
00110 MatrixXd  H(matrixSMAX,matrixSMAX);                                             // Matrice/ Observation Systeme
00111 //Definition/ Variables Globales/ Processus Principal/ Variables Matricielles/ Covariance 
00112 MatrixXd  Q(matrixSMAX,matrixSMAX);                                             // Matrice/ Covariance Bruit Systeme (perturbations)
00113 MatrixXd  R(matrixSMAX,matrixSMAX);                                             // Matrice/ Covariance Bruit Mesure
00114 //Definition/ Variables Globales/ Processus Principal/ Variables Matricielles/ Caractérisation du Filtre Kalman
00115 MatrixXd pXk(matrixSMAX,1);                                                     // Vecteur/ Etat a priori
00116 MatrixXd pPk(matrixSMAX,matrixSMAX);                                            // Matrice/ Erreur Covariance a prori
00117 MatrixXd  Xk(matrixSMAX,1);                                                     // Vecteur/ Etat 
00118 MatrixXd  Pk(matrixSMAX,matrixSMAX);                                            // Matrice/ Erreur Covariance
00119 MatrixXd  Kk(matrixSMAX,matrixSMAX);                                            // Matrice/ Gain Kalman
00120 MatrixXd  Zk(matrixSMAX,1);                                                     // Vecteur/ Mesure
00121 MatrixXd  Uk(matrixSMAX,1);                                                     // Vecteur/ Commande
00122 MatrixXd I= MatrixXd::Identity(matrixSMAX,matrixSMAX);                          // Matrice/ Identité
00123 
00124 
00125 
00126 
00127 //Definition/ Variables Globales/ Processus Principal/ Variables Matricielles/ Identification Processus
00128 float An= 1,
00129       Bn= 0,
00130       Hn= 1,
00131       Qn= 0.00001,
00132       Rn= 0.1,
00133       pXn= 0,
00134       pPn= 0,
00135       Xn= 0,
00136       Yn= 0,
00137       Sn= 0,
00138       Pn= 1,
00139       Kn= 0,
00140       Zn= 0,
00141       Un= 0,
00142       In= 1;
00143 
00144 
00145 //Definition/ Procedures/ Reception UART1 (Interruption)
00146 //==============================================================================
00147 void Rx_UART1_Isr(void){ 
00148 
00149 char RxCar; 
00150 int pCar;
00151 int r1, r2, r3, r4, r5;
00152 
00153 RxCar   = UART1.getc();
00154 UART1Rx+= RxCar;
00155 
00156 USB.printf("%c", RxCar);
00157 
00158 if(UART1Rx.find("\r\n")< UART1Rx.length()){                                     //Balise fin de trame
00159       if(UART1Rx.find("$,123")< UART1Rx.length()){
00160         l2WDT= 3;
00161         
00162 /*        ++cnt; if(cnt> NbEch){cnt= 0;};
00163         signal= 900* cos(((6.28* cnt)/NbEch)/(5.32*periode))* cos(((6.28* cnt)/NbEch)/periode);
00164         r1= signal;
00165 */      
00166         r1= (int)imu.roll;  if(r1> 1000){r1= 1000;}; if(r1< -1000){r1= -1000;};
00167         r2= (int)imu.pitch; if(r2> 1000){r2= 1000;}; if(r2< -1000){r2= -1000;};
00168         r3= (int)imu.yaw; if(r3> 1000){r3= 1000;}; if(r3< -1000){r3= -1000;};
00169         r4= 0;
00170         r5= 0;
00171         sprintf(UART1Tx,"$,123,%d,%d,%d,%d,%d,*\r\n",r1,r2,r3,r4,r5);             
00172         UART1.printf("%s",UART1Tx); 
00173         USB.printf("%s",UART1Tx);   
00174        };                       
00175     UART1Rx= "";
00176     }; // fin if(UART2Rx.find("\r\n")< UART2Rx.length()){  
00177 }   
00178 
00179 
00180 
00181 
00182 
00183 //Definition/ Procedures/ Reception UART2 (Interruption)
00184 //==============================================================================
00185 void Rx_UART2_Isr(void){ 
00186 
00187 char RxCar; 
00188 int pCar;
00189 int    fx, nt;
00190 float  vg, hd, dp;
00191 double ut, lt, lg, at, deg, min, dmin;
00192 string id, valid, ns, ew;
00193 
00194 RxCar   = UART2.getc();
00195 UART2Rx+= RxCar;
00196 
00197 if(UART2Rx.find("\r\n")< UART2Rx.length()){                                     //Balise fin de trame
00198     l1WDT= 3;
00199  
00200     pCar= 0;while(pCar< UART2Rx.length()){if(UART2Rx[pCar]== ','){UART2Rx.replace(pCar,1," ");};++pCar;};  //message: suppression des caracters ','
00201 
00202     // Trame NMEA-083: $GPRMC
00203     if(UART2Rx.find("$GPRMC")< UART2Rx.length()){                               // Example Trame: $GPRMC,225446,A,4916.45,N,12311.12,W,000.5,054.7,191194,020.3,E*68
00204             ++cT; if(cT> 32000){cT= 0;};                                        // Totalisateur de Trame
00205             sprintf(gpsNMEA,"%s/ Trame NMEA (%d) :  %s \r\n",produit,cT,UART2Rx); prn= true;
00206 //            USB.printf("%s/ Trame NMEA (%d) :  %s \r\n",produit, cT,UART2Rx);          //DEBUG/ USB 
00207             istringstream input(UART2Rx); 
00208             input>> id>> ut>> valid>> lt>> ns>> lg>> ew>> vg>> hd;
00209 //            USB.printf("%s/ Trame NMEA/ id   :  %s \r\n",produit, id);                 // GPS/ id
00210  //           USB.printf("GPS/ Trame NMEA/ val  :  %s \r\n", valid);              // GPS/ valid
00211  //           USB.printf("GPS/ Trame NMEA/ utc  :  %f \r\n", ut);                 // GPS/ id            
00212             // Convertion NED ---> ECEF (MERCATOE WGS84)
00213             gps.x= 0;
00214             gps.y= 0;
00215             gps.z= 0;                                
00216             if(valid== "A"){                                                    //GPS/ data trame NMEA valides
00217                   gpsWDT= gpsWDTM; 
00218                   gps.fValid= true; 
00219 //                  USB.printf("%s/ Trame NMEA/ val  :  %s \r\n",produit, valid);        // GPS/ valid
00220 //                  USB.printf("%s/ Trame NMEA/ utc  :  %f \r\n",produit, ut);           // GPS/ id                                                                      
00221                   };
00222             if(gpsWDT== 0){
00223 //                  USB.printf("%s/ Trame NMEA/ val  :  INVALIDE !!! \r\n",produit);
00224                   };     // GPS/ valid
00225                   
00226 //             USB.printf("\r\n\r\n\r\n");      
00227              }; // if(UART2Rx.find("$GPRMC")< UART2Rx.length()){
00228                           
00229     UART2Rx= "";
00230     }; // fin if(UART2Rx.find("\r\n")< UART2Rx.length()){  
00231 }   
00232 
00233 
00234 
00235 
00236 
00237 
00238 //Definition/ Procedures/ Reception des Trames serie (USB-UART Interruption)
00239 //==============================================================================
00240 void Rx_USB_Isr(void){ 
00241     int cnt= 0;
00242     char car= USB.getc();
00243     
00244     switch((int)car){
00245       
00246       case  97: //Touche 'a'
00247                 break;
00248       case 122: //Touche 'z'
00249                 break;                 
00250       case  BS://Back Space
00251                if(USBRx.length()> 0){
00252                   USBRx.erase(USBRx.length()-1,1);
00253                   while(cnt< USBRx.length()){USB.printf(" ");++cnt;};
00254                   USB.printf("\r%s",USBRx);
00255                   };
00256                break;
00257       case LF: //Line Feed '\n'
00258              break;
00259       case CR: //Carriage Return '\r'
00260              USBRx+= "\r\n";
00261              USB.printf("\r\n"); 
00262              USBRx= "";
00263              break;
00264       default: //Caractere ASCII
00265              USBRx+= car;
00266              USB.printf("%c",car);
00267              break;
00268       }; // fin switch((int)car){
00269    
00270     }    
00271 
00272 
00273 
00274 
00275 
00276 
00277 
00278 
00279 
00280 //Definition/ Procedures/ Processus Principal (Interruption)
00281 //==============================================================================
00282 void MainProcess_Isr(void){
00283     //Watchdogs
00284     --l1WDT;  if(l1WDT<= 0){l1WDT= 0; l1= OFF;}else{l1= ON;};
00285     --l2WDT;  if(l2WDT<= 0){l2WDT= 0; l2= OFF;}else{l2= ON;};
00286     --l3WDT;  if(l3WDT<= 0){l3WDT= 0; l3= OFF;}else{l3= ON;};
00287     --l4WDT;  if(l4WDT<= 0){l4WDT= 0; l4= OFF;}else{l4= ON;};
00288     --imuWDT; if(imuWDT<= 0){imuWDT= 0;};
00289     --gpsWDT; if(gpsWDT<= 0){
00290                              gpsWDT= 0;
00291                              gps.fValid= false; 
00292                              gps.latitude= 0; gps.longitude= 0; gps.altitude= 0; 
00293                              gps.heading= 0; gps.vground= 0;};
00294     
00295     //I2C/ imu
00296     if((imuWDT== 0)& (imu.fValid== true)){
00297        attitude.get_calib();
00298                   
00299        attitude.get_lia();                                                      // I2C/ IMU/ Acceleration Lineaire
00300        imu.liax = attitude.lia.x;
00301        imu.liay = attitude.lia.y;
00302        imu.liaz = attitude.lia.z;
00303        
00304     
00305        Zn= imu.roll;  
00306 /*     
00307        // Filtre Kalman/ Algorithme/ Prediction 
00308        pXn = (An * Xn) + (Bn * Un);                                             // 1- Equation Etat
00309        pPn = (An * Pn * An) + Qn;                                               // 2- Equation Covariance de l'erreur
00310        // Filtre Kalman/ Algorithme/ Correction 
00311        Kn = pPn * (Hn / ((Hn * pPn * Hn) + Rn));                                // 1- Mise a jour Gain Kalman
00312        Xn = pXn + (Kn * (Zn - (Hn * pXn)));                                     // 2- Mise a jour Etat
00313        Pn = (In - (Kn * Hn)) * pPn;                                             // 3- Mise à jour Covariance de l'erreur   
00314 */       
00315        
00316        pXn = Xn; 
00317        pPn = Pn + Qn;
00318        Yn  = Zn - pXn;
00319        Sn  = pPn+ Rn;
00320        Kn  = pPn/ Sn;
00321        Xn  = pXn+ Kn*Yn;
00322        Pn  = (In- Kn)*pPn;
00323                       
00324        attitude.get_grv();                                                      // I2C/ IMU/ Acceleration Gravite
00325        imu.grvx = attitude.gravity.x;
00326        imu.grvy = attitude.gravity.y;
00327        imu.grvz = attitude.gravity.z;
00328  
00329        attitude.get_gyro();                                                     // I2C/ IMU/ Gyroscope
00330        imu.gyrox = attitude.gyro.x;
00331        imu.gyroy = attitude.gyro.y;
00332        imu.gyroz = attitude.gyro.z;
00333                   
00334        attitude.get_mag();                                                      // I2C/ IMU/ Champ Magnetique
00335        imu.magx = attitude.mag.x;
00336        imu.magy = attitude.mag.y;
00337        imu.magz = attitude.mag.z;
00338                                     
00339        attitude.get_angles();                                                   // I2C/ IMU/ Angles EULER
00340        imu.roll = attitude.euler.roll;
00341        imu.pitch= attitude.euler.pitch;
00342        imu.yaw  = attitude.euler.yaw;
00343                   
00344        attitude.get_quat();                                                     // I2C/ IMU/ Quarternions
00345        imu.q0   = attitude.quat.w; 
00346        imu.q1   = attitude.quat.x;
00347        imu.q2   = attitude.quat.y;
00348        imu.q3   = attitude.quat.z;
00349                   
00350        attitude.get_temp();                                                     // I2C/ IMU/ Temperature
00351        imu.temperature = attitude.temperature; 
00352              
00353        imuWDT= 20;   
00354        
00355      };//fi if((imuWDT== 0)& (imu.fValid== true)){
00356 
00357 /*               
00358     //Localisation/ Fusion GPS-ACCELEROMETRE/ Fitrage Kalman (ref.2, ref.3)
00359     //Filtre/ Entree   : Uk (vecteur Commande)
00360     //Filtre/ Entree   : Zk (Vecteur Observabiité)
00361     //Filtre/ Sortie   : Xk (Vecteur Etat)
00362     //Filtre/ Sortie   : Pk (Vecteur Covariance)
00363     //Filtre/ Parametre: A (Matrice Etat)
00364     //Filtre/ Parametre: B (Matrice Commande)
00365     //Filtre/ Parametre: H (Matrice Observabilité)
00366     //Filtre/ Parametre: Q (Matrice Covariance Etat (bruit))
00367     //Filtre/ Parametre: R (Matrice Covariance Observabilité (bruit))   
00368    
00369     // Filtre Kalman/ Algorithme/ Prediction 
00370     pXk = (A * Xk) + (B * Uk);                                                  // 1- Equation Etat
00371     pPk = (A * Pk * A.transpose()) + Q;                                         // 2- Equation Covariance de l'erreur
00372     // Filtre Kalman/ Algorithme/ Correction 
00373     Kk = pPk * H.transpose() * ((H * pPk * H.transpose()) + R).inverse();       // 1- Mise a jour Gain Kalman
00374     Xk = pXk + (Kk * (Zk - (H * pXk)));                                         // 2- Mise a jour Etat
00375     Pk = (I - (Kk * H)) * pPk;                                                  // 3- Mise à jour Covariance de l'erreur       
00376 */
00377   
00378     }
00379 
00380 
00381 
00382 
00383 
00384 
00385 
00386 //Definition/ Procedures/ Initialisation
00387 //==============================================================================
00388 int init_process(void){
00389     int status= 0;
00390     USB.baud(usbBR); wait(0.3); 
00391     UART1.baud(uartBR); wait(0.3);  
00392     UART2.baud(gpsBR); wait(0.3); 
00393     i2c.frequency(i2cBR); wait(0.2);  
00394     
00395         // Initialisation/  Objet/ GPS
00396     gps.fValid             = false;
00397     gps.wdtRX              = gpsWDT;
00398     gps.baud               = gpsBR;
00399  //   gps.TSample            = 5; 
00400     gps.utc                = 0;
00401     gps.vground            = 0;
00402     gps.heading            = 0;
00403     gps.fix                = 0;
00404     gps.nbsat              = 0;
00405     gps.dop                = 0;
00406     gps.altitude           = 0;
00407     gps.latitude           = 0;
00408     gps.longitude          = 0;
00409     gps.x                  = 0;
00410     gps.y                  = 0;
00411     gps.z                  = 0;
00412     gps.speedmin           = 5;
00413     gps.Status             = 0;   
00414 //   USB.printf("."); wait(0.3);
00415                            
00416     // Initialisation/ Type/ objet IMU  
00417     imu.fValid             =  true;
00418     imu.fFailed            =  false;
00419     imu.Status             =  0;  
00420     imu.roll               = 0;   
00421     imu.pitch              = 0;   
00422     imu.yaw                = 0;  
00423     imu.q0                 = 1;
00424     imu.q1                 = 0;
00425     imu.q2                 = 0;
00426     imu.q3                 = 0;
00427     imu.liax               = 0;    
00428     imu.liay               = 0;    
00429     imu.liaz               = 0;      
00430     imu.grvx               = 0;    
00431     imu.grvy               = 0;    
00432     imu.grvz               = 0;    
00433     imu.gyrox              = 0;
00434     imu.gyroy              = 0;
00435     imu.gyroz              = 0;
00436     imu.magx               = 0;
00437     imu.magy               = 0;
00438     imu.pressure           = 0; 
00439     imu.humidity           = 0;
00440     imu.temperature        = 0; 
00441     imu.altitude           = 0;  
00442     imu.magz               = 0;
00443     imu.Status             = 0;
00444     if(imu.fValid== true){
00445         attitude.reset(); wait(1);
00446         if (!attitude.check()){                                                 // Presence Capteur Attitude ?
00447              while (1){l1 =!l1; l2 =!l2; l3 =!l3; l4 =!l4; son= !son; wait(0.3);};
00448             };                                                                  // Arret et flash LED et SOUND si le capteur d'attitude n'est pas reconnu
00449 //        attitude.setpowermede();                                              // A definir
00450         attitude.set_accel_units(MILLIG);                                       // Unite Accelleration MILLIG ou MPERSPERS
00451         attitude.set_anglerate_units(DEG_PER_SEC);                              // Unite Vitesse Angulaire RAD_PER_SEC ou DEG_PER_SEC
00452         attitude.set_angle_units(RADIANS);                                      // Unite Angle RADIANS ou DEGREES
00453         attitude.set_temp_units(CENTIGRADE);                                    // Unite Temperature CENTIGRADE ou FAHRENHEIT
00454         attitude.setmode(OPERATION_MODE_NDOF);                                  // Mode Fonctionnement
00455         
00456 //        if (pression.init() != 0) {                                             // Presence Capteur pression atmosphérique ?
00457 //             while (1){L1 =!L1; L2 =!L2; L3 =!L3; L4 =!L4; SOUND= !SOUND; wait(0.3);};
00458 //            }; 
00459                                                                              // Arret et flash LED et SOUND si le capteur de pression n'est pas reconnu
00460         }; // fin if(imu.fvalid== true){
00461  //   USB.printf("."); wait(0.3);
00462 
00463     
00464     // Initialisation/ Objet/ MATRICE       
00465     int cntr=0, cntc= 0;
00466     cntr=0, cntc= 0;
00467     while(cntr< matrixSMAX){while(cntc< matrixSMAX){A(cntr,cntc)= 0;++cntc;};++cntr;};
00468     cntr=0, cntc= 0;
00469     while(cntr< matrixSMAX){while(cntc< matrixSMAX){B(cntr,cntc)= 0;++cntc;};++cntr;};
00470     cntr=0, cntc= 0;
00471     while(cntr< matrixSMAX){while(cntc< matrixSMAX){H(cntr,cntc)= 0;++cntc;};++cntr;};
00472     cntr=0, cntc= 0;
00473     while(cntr< matrixSMAX){while(cntc< matrixSMAX){Q(cntr,cntc)= 0;++cntc;};++cntr;};
00474     cntr=0, cntc= 0;
00475     while(cntr< matrixSMAX){while(cntc< matrixSMAX){R(cntr,cntc)= 0;++cntc;};++cntr;};  
00476     cntr=0, cntc= 0;
00477     while(cntr< matrixSMAX){while(cntc< 1){pXk(cntr,cntc)= 0;++cntc;};++cntr;};
00478     cntr=0, cntc= 0;
00479     while(cntr< matrixSMAX){while(cntc< matrixSMAX){pPk(cntr,cntc)= 0;++cntc;};++cntr;};
00480     cntr=0, cntc= 0;
00481     while(cntr< matrixSMAX){while(cntc< 1){Xk(cntr,cntc)= 0;++cntc;};++cntr;};
00482     cntr=0, cntc= 0;
00483     while(cntr< matrixSMAX){while(cntc< matrixSMAX){Pk(cntr,cntc)= 0;++cntc;};++cntr;};
00484     cntr=0, cntc= 0;
00485     while(cntr< matrixSMAX){while(cntc< matrixSMAX){Kk(cntr,cntc)= 0;++cntc;};++cntr;};
00486     cntr=0, cntc= 0;
00487     while(cntr< matrixSMAX){while(cntc< 1){Zk(cntr,cntc)= 0;++cntc;};++cntr;};
00488     cntr=0, cntc= 0;
00489     while(cntr< matrixSMAX){while(cntc< 1){Uk(cntr,cntc)= 0;++cntc;};++cntr;};
00490     
00491     // Initialisation/ Objet/ Matrice Etat/ A
00492     A<< 1, TSample, 0, 0,
00493         0, 1      , 0, 0,
00494         0, 0      , 0, TSample,
00495         0, 0      , 0, 1;
00496              
00497     // Initialisation/ Objet/ Matrice Commande/ B
00498     B<< 1, 0, 0, 0,
00499         0, 1, 0, 0,
00500         0, 0, 1, 0,
00501         0, 0, 0, 1;
00502  
00503     // Initialisation/ Objet/ Matrice Observabilite (Mesure)/ H
00504     H<< 1, 1, 0, 0,
00505         0, 1, 0, 0,
00506         0, 0, 1, 0,
00507         0, 0, 0, 1;
00508    
00509     // Initialisation/ Objet/ Matrice Covariance Etat/ P
00510     Pk<< 1, 0, 0, 0,
00511          0, 1, 0, 0,
00512          0, 0, 1, 0,
00513          0, 0, 0, 1;
00514         
00515     // Initialisation/ Objet/ Matrice Covariance Bruit Etat/ Q
00516     Q<< 0, 0, 0, 0,
00517         0, 0, 0, 0,
00518         0, 0, 0, 0,
00519         0, 0, 0, 0;
00520         
00521     // Initialisation/ Objet/ Matrice Covariance Bruit Observabilité (Mesure)/ R
00522     R<< 0.2, 0,   0,   0,
00523         0,   0.2, 0,   0,
00524         0,   0,   0.2, 0,
00525         0,   0,   0,   0.2;
00526  
00527     // Initialisation/ Objet/ Matrice Identité/ I
00528     I<<  1, 0, 0, 0,
00529          0, 1, 0, 0,
00530          0, 0, 1, 0,
00531          0, 0, 0, 1;
00532        
00533     TMainProcess.attach(&MainProcess_Isr, TSample); 
00534     UART1.attach(&Rx_UART1_Isr, Serial::RxIrq);
00535     UART2.attach(&Rx_UART2_Isr, Serial::RxIrq);
00536     USB.attach(&Rx_USB_Isr, Serial::RxIrq);
00537     son= 0;
00538     l1= OFF; l2= OFF; l3= OFF; l4= OFF;
00539     USB.printf("%s/ Essai ...\r\n",produit); wait(3);
00540     return status;
00541     }
00542 
00543 
00544 
00545 
00546 
00547 
00548 
00549 //Definition/ Procedures/ Procedure Principale
00550 //==============================================================================
00551 int main() {
00552     int status= 0;
00553     if(init_process()!= 0){l1=1, l2= 1; l3= 1; l4= 1; wait(0.3); l1=0, l2= 0; l3= 0; l4= 0; wait(0.7);};     
00554     while(1){};
00555     return status;
00556     }