Travaux Pratiques » Polytech Ge SP4abc 2021

}/*

}*/

char caract='5';

int valeur;

valeur = decode_int(caract);

printf("La valeur en decimale est : %d\n",valeur);

/*void test_decode_nombre(void)

}*/

u0tb = c;

}

smd2_u0mr = 1;

u0brg = 0x20; // baud rate

clk1_u0c0 =0;

crs_u0c0 =0;

txept_u0c0 =0;

crd_u0c0 =1;

nch_u0c0 =0;

ckpol_u0c0 =0;

uform_u0c0 =0;

te_u0c1 = 1;

re_u0c1 = 1;

ri_u0c1 = 0;

u0irs_u0c1 = 0;

u0rrm_u0c1 = 0;

sclkstpb_u0c1 = 0;

c = 'A';

}

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;

}