Travaux Pratiques » Polytech Ge SP4abc 2021

end_comp=type[i];v;

if (decode_nombre("7541",1)!=7)https://forge.clermont-universite.fr/documents/756

{

printf("Erreur test unitaire decode nombre\n");

exit(-1);

}

if (decode_nombre("7541",2)!=75)

{

printf("Erreur test unitaire decode nombre\n");

exit(-1);

}

if (decode_nombre("7541",3)!=754)

{

printf("Erreur test unitaire decode nombre\n");

exit(-1);

}

}

printf("Erreur test unitaire decode nombre\n");

exit(-1);

}

if (decode_nombre("7541",3)!=754)

{

printf("Erreur test unitaire decode nombre\n");

exit(-1);

}

}

exit(-1);

}

}

// Ne pas modifier cette fonction

int main(int argc,char ** argv)

{

tests_unitaires();

// Affichage des trames definies dans la table trames.

printf ("Trames de tests tableau trames:\n");

int i=0;

while (trames[i])

traitement(trames[i++]);

if (!trame_init())

exit(-1);

// Affichage des trames du fichier gps.log

char *trame;

printf ("Trames de tests du fichier gps.log\n");

while ((trame = trame_suivante()))

traitement(trame);

return 0;

}

/ Pour compiler : gcc sp4a3_kalman.c -lm

#include <stdlib.h>

#include <stdio.h>

#include <math.h>

#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]){

int I,J=0;

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 B[n][m]){

float d;

d=1/((A[0][0]*A[1][1])-(A[0][1]*A[1][0]));

B[0][0]=d*A[1][1];

B[1][1]=d*A[0][0];

B[0][1]=-d*(A[0][1]);

B[1][0]=-d*(A[1][0]);

}

void Transpose_Mat(int n,int m,double A[n][m],double R[m][n]){

int i,j;

for (i=0;i<n;i++)

for (j=0;j<m;j++)

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]){

int I,J=0;

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]){

int i=0;

int j=0;

int k=0;

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];

}

}

}

}

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");

}

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/ Pour compiler : gcc sp4a3_kalman.c -lm

#include <stdlib.h>

#include <stdio.h>

#include <math.h>

#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]){

int I,J=0;

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 B[n][m]){

float d;

d=1/((A[0][0]*A[1][1])-(A[0][1]*A[1][0]));

B[0][0]=d*A[1][1];

B[1][1]=d*A[0][0];

B[0][1]=-d*(A[0][1]);

B[1][0]=-d*(A[1][0]);

}

void Transpose_Mat(int n,int m,double A[n][m],double R[m][n]){

int i,j;

for (i=0;i<n;i++)

for (j=0;j<m;j++)

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]){

int I,J=0;

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]){

int i=0;

int j=0;

int k=0;

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];

}

}

}

}

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");

}

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=0; ///Mettre ? 1 pour afficher les matrices.

///Ajouter votre code ci-dessous///

// Kalman

// X = F*X

Plot_Mat(X," X(k+1|k) = ");

//P = F*P*F'+Q;

Plot_Mat(P,"P(k+1|k) = F.P(k|k).FT + Q = ");

// K = P*H' / ( H*P*H' + R);

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 = ");

// P = P - K*H*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 ///

}

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;

}},

{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;/ Pour compiler : gcc sp4a3_kalman.c -lm

#include <stdlib.h>

#include <stdio.h>

#include <math.h>

#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]){

int I,J=0;

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 B[n][m]){

float d;

d=1/((A[0][0]*A[1][1])-(A[0][1]*A[1][0]));

B[0][0]=d*A[1][1];

B[1][1]=d*A[0][0];

B[0][1]=-d*(A[0][1]);

B[1][0]=-d*(A[1][0]);

}

void Transpose_Mat(int n,int m,double A[n][m],double R[m][n]){

int i,j;

for (i=0;i<n;i++)

for (j=0;j<m;j++)

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]){

int I,J=0;

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]){

int i=0;

int j=0;

int k=0;

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];

}

}

}

}

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}};

/ Pour compiler : gcc sp4a3_kalman.c -lm

#include <stdlib.h>

#include <stdio.h>

#include <math.h>

#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]){

int I,J=0;

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 B[n][m]){

float d;

d=1/((A[0][0]*A[1][1])-(A[0][1]*A[1][0]));

B[0][0]=d*A[1][1];

B[1][1]=d*A[0][0];

B[0][1]=-d*(A[0][1]);

B[1][0]=-d*(A[1][0]);

}

void Transpose_Mat(int n,int m,double A[n][m],double R[m][n]){

int i,j;

for (i=0;i<n;i++)

for (j=0;j<m;j++)

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]){

int I,J=0;

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]){

int i=0;

int j=0;

int k=0;

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];

}

}

}

}

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");

}

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=0; ///Mettre ? 1 pour afficher les matrices.

///Ajouter votre code ci-dessous///

// Kalman

// X = F*X

Plot_Mat(X," X(k+1|k) = ");

//P = F*P*F'+Q;

Plot_Mat(P,"P(k+1|k) = F.P(k|k).FT + Q = ");

// K = P*H' / ( H*P*H' + R);

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 = ");

// P = P - K*H*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 ///

}

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;

}

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");

}

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}};

#include <stdio.h>

#include <stdlib.h>

#include <strings.h>

#include "trame.h"

#include <math.h>

typedef struct // declaration du type position compos? de la latitude et la longitude

{

float latitude;

float longitude;

} position;

typedef struct // declaration du type zone compos? de la position et la vitesse maximale

{

position rpos;

float vitmax;

} zone;

#define Rayon 6378.1370

//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",

"$GPGSV,3,3,10,22,39,053,50,28,15,320,*7E",

"$GPRMC,141914.00,A,4545.6424,N,00306.6036,E,0.4,99.4,010206,,*0C",

"$GPGLL,4545.6424,N,00306.6036,E,141914.00,A*0E",

"$GPGGA,141914.00,4545.0000,N,00306.6036,E,1,05,3.4,499.3,M,,M,,*7D",

"$GPGGA,141925.00,4545.2410,N,00306.6046,E,1,05,3.4,501.4,M,,M,,*7D"

"$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};

zone zones[] = // declaration d'un tableau de zones

{

{{44.78887762, -3.012}, 50},

{{44.7891220, -3.013}, 70},

{{46.7501,3.1101},50}

};

position test_position;

position pos1;

position pos2;

int trame_cmp(char * trame, char * type) // declaration de la fonction de comparaison de deux chaines

{

/* if (!strncmp(trame+1,type,strlen(type)))

return 1;

return 0;

*/

int i;

for (i=0; i<strlen(type); i++) // parcours de la longueur du type

{

if (trame[i+1] != type[i]) // inegalit? entre les caracteres

return 0; // renvoie 0

}

return 1; // renvoie 1

}

int decode_int(char c) // declaration de la fonction qui renvoie le nombre associ? au caractere

{

c=c-'0';

return c;

}

int decode_nombre(char * ch, int n) // fonction qui recupere les n premiers chiffres d'une chaine

{

int i;

int somme=0;

for (i=0;i<n;i++)

{

somme=(somme*10) + decode_int(ch[i]);

}

return somme;

}

float conversion_longitude(char * lon) // fonction qui convertie une longitude en nombre flottant

{

float result_convers,val_deci;

int val_ent_dec,val_ent_minut;

result_convers=decode_nombre(lon,3);// recuperation de la partie entiere

val_ent_minut=decode_nombre(&lon[3],2);

val_ent_dec=decode_nombre(&lon[6],4);

val_deci=(val_ent_minut+(0.0001*val_ent_dec))/60; // recuperation de la valeur decimale en degr?

result_convers=result_convers+val_deci;

return result_convers;

}

float conversion_latitude(char * lat) // fonction qui convertie une longitude en nombre flottant

{

// 3723.2475 equivalent a 37 degr? 23.2475 / 60 degr?

float result_convers,val_deci;

int val_ent_dec,val_ent_minut;

char minute[7];

char min[4];

int i;

for (i=0;i<7; i++) minute[i]=lat[2+i];

for (i=0;i<4; i++) min[i]=lat[5+i];

result_convers=decode_nombre(lat,2);// recuperation de la partie entiere 37

val_ent_minut=decode_nombre(minute,2); // 23

val_ent_dec=decode_nombre(min,4); // 2475

val_deci=(val_ent_minut+(val_ent_dec*0.0001))*0.0166667; // recuperation de la valeur decimale en degr?

result_convers+=val_deci;

return result_convers;

}

int decode_trame(char* trame,position * pos) // fonction qui rends les trames "GPGGA" et qui renvoie 1 et affichage de la latitude et la longitude en floattant

{

int i;

char ch[10];

if (trame_cmp(trame,"GPGGA"))

{

for (i=0;i<10;i++) ch[i]=trame[29+i];

pos->latitude=conversion_latitude(trame+17);

pos->longitude=conversion_longitude(ch);

return 1;

}

return 0;

}

float conver_radian(float x) // fonction qui convertit un nombre flottant en flottaant

{

return (x*M_PI)/180;

}

float calcul_distance(position pos1,position pos2) // fonction qui calcule la distance entre deux positions

{

float result= (40000/360) * sqrt(((pos2.latitude - pos1.latitude)*(pos2.latitude - pos1.latitude))+((pos2.longitude - pos1.longitude)*(pos2.longitude - pos1.longitude))) ;

return result;

}

float calcul_vitesse(position pos1,position pos2) // fonction qui calcule la distance entre deux positions

{

float vitesse;

vitesse=3600*(calcul_distance(pos1,pos2)*Rayon);

return vitesse;

}

int distance_a_la_plus_proche_zone(position p,zone r[],int nb_zones,float *d) // fonction qui calcule la distance la plus proche

{

int index=0;

int i;

*d=fabs(calcul_distance(p,r[0].rpos)); // initialisation du minimum

float t;

if (nb_zones>0)

{

for (i=1;i<nb_zones;i++)

{

t=fabs(calcul_distance(p,r[i].rpos));

if (t<*d)

{

*d=t;

index=i;

}

}

return index;

}

return -1;

}

void test_distance_plus_proche(void) //

{

position p;

p.latitude =45.75;

p.longitude=3.110060;

int num_zone;

float d ;

num_zone=distance_a_la_plus_proche_zone(p,zones,sizeof(zones),&d);

if (num_zone!=2 & fabs(d-44.85)>0.5)

{

printf(("erreur test distance la plus proche "));

exit(-1);

}

}

float fonction_generique(char* ch) // fonction qui convertit la latitudes ou la longitude en flottant

{

float result_convers;

if(ch[4]=='.')

{

result_convers=conversion_latitude(ch);

}

else if(ch[5]=='.')

{

result_convers=conversion_longitude(ch);

}

return result_convers;

}

//Fonction ? modifier !!!!!

void traitement(char * trame)

{

/*

static int cpt=0;

cpt++;

if (trame_cmp(trame,"GPGGA"))

{printf ("> %s\n",trame);}

*/

position p;

position p_init;

p_init.latitude=45.750000;

p_init.longitude= 3.110060;

float distance;

float vitesse;

float seuil=1000;

int index;

if (decode_trame(trame,&p))

{

printf ("> %s\n",trame);

printf("la latitude est %f \n la longitude est %f \n ",p.latitude,p.longitude);

vitesse=calcul_vitesse(p_init,p);

index=distance_a_la_plus_proche_zone(p,zones,sizeof(zones),&distance);

if ((distance<seuil) && (vitesse>zones[index].vitmax))

{

printf(" alarme on \n");

printf(" zone numero %d \n ",index);

printf("la vitesse est %f\n ",vitesse);

printf("la distance est %f \n ",distance);

}

else

printf("Alarme off \n ");

p_init = p;

}

}

void test_decode_trame(void)

{

if (decode_trame("$GPGGA,141914.00,4545.0000,N,00306.6036,E,1,05,3.4,499.3,M,,M,,*7D",&test_position)!=1)

{

printf("Erreur Test decode trame \n");

exit(-1);

}

}

void test_decode_int(void)

{

if (decode_int('0')!=0)

{

printf("Erreur test unitaire decode int\n");

exit(-1);

}

if (decode_int('1')!=1)

{

printf("Erreur test unitaire decode int\n");

exit(-1);

}

if (decode_int('2')!=2)

{

printf("Erreur test unitaire decode int\n");

exit(-1);

}

if (decode_int('3')!=3)

{

printf("Erreur test unitaire decode int\n");

exit(-1);

}

if (decode_int('4')!=4)

{

printf("Erreur test unitaire decode int\n");

exit(-1);

}

if (decode_int('5')!=5)

{

printf("Erreur test unitaire decode int\n");

exit(-1);

}

}

void test_decode_nombre(void)

{

void test_fonction_generique(void)

{

if (fonction_generique("3223.2475") - 37.387459 > 0.0001 )

{

printf("Erreur test latitude !!! \n");

exit(-1);

}

if (fonction_generique("00306.6036")-3.110060 > 0.0001)

{

printf("Erreur test longitude !!! \n");

exit(-1);

}

}

void test_latitude(void)

{

if (conversion_latitude("3223.2475,N") - 37.387459 > 0.0001 )

{

printf("Erreur test latitude !!! \n");

exit(-1);

}

}

void test_longitude(void)

{

if (conversion_longitude("00306.6036,E")-3.110060 > 0.0001)

{

printf("Erreur test longitude !!! \n");

exit(-1);

}

}

//Ajouter vos tests unitaires dans cette fonction.

void tests_unitaires(void)

{

if (5!=5){

printf ("Erreur Test unitaire basique.\n");

exit(-1);

}

if (trame_cmp("$GPGGA suite chaine","GPGGA")!=1){

printf ("Erreur Test unitaire trame_cmp.\n");

exit(-1);

}

if (trame_cmp("$GPRMC suite chaine","GPGGA")!=0){

printf ("Erreur Test unitaire trame_cmp.\n");

exit(-1);

}

if (trame_cmp("$GPRMC... ", "GPRMC" )!=1){

printf ("Erreur Test unitaire trame_cmp.\n");

exit(-1);

}

if (trame_cmp("$APRMC...", "GPGGA")!=0){

printf ("Erreur Test unitaire trame_cmp.\n");

exit(-1);

}

}

// Ne pas modifier cette fonction

int main(int argc,char ** argv)

{

int trame_valide;

tests_unitaires();

test_decode_int();

test_decode_nombre();

test_latitude();

test_longitude();

test_fonction_generique();

test_decode_trame();

test_distance_plus_proche();

// Affichage des trames definies dans la table trames.

printf ("Trames de tests tableau trames:\n");

int i=0;

while (trames[i])

traitement(trames[i++]);

if (!trame_init())

exit(-1);

// Affichage des trames du fichier gps.log

char *trame;

printf ("Trames de tests du fichier gps.log\n");

while ((trame = trame_suivante()))

traitement(trame);

return 0;

}

double Q[4][4] = {{0, 0, 0, 0},

/ Pour compiler : gcc sp4a3_kalman.c -lm

#include <stdlib.h>

#include <stdio.h>

#include <math.h>

#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]){

int I,J=0;

for (I=0; I<na; I++){

for (J=0; J<ma; J++){#include <stdio.h>

#include <stdlib.h>

#include <strings.h>

#include "trame.h"

#include <math.h>

typedef struct // declaration du type position compos? de la latitude et la longitude

{

float latitude;

float longitude;

} position;

typedef struct // declaration du type zone compos? de la position et la vitesse maximale

{

position rpos;

float vitmax;

} zone;

#define Rayon 6378.1370

//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",

"$GPGSV,3,3,10,22,39,053,50,28,15,320,*7E",

"$GPRMC,141914.00,A,4545.6424,N,00306.6036,E,0.4,99.4,010206,,*0C",

"$GPGLL,4545.6424,N,00306.6036,E,141914.00,A*0E",

"$GPGGA,141914.00,4545.0000,N,00306.6036,E,1,05,3.4,499.3,M,,M,,*7D",

"$GPGGA,141925.00,4545.2410,N,00306.6046,E,1,05,3.4,501.4,M,,M,,*7D"

"$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};

zone zones[] = // declaration d'un tableau de zones

{

{{44.78887762, -3.012}, 50},

{{44.7891220, -3.013}, 70},

{{46.7501,3.1101},50}

};

position test_position;

position pos1;

position pos2;

int trame_cmp(char * trame, char * type) // declaration de la fonction de comparaison de deux chaines

{

/* if (!strncmp(trame+1,type,strlen(type)))

return 1;

return 0;

*/

int i;

for (i=0; i<strlen(type); i++) // parcours de la longueur du type

{

if (trame[i+1] != type[i]) // inegalit? entre les caracteres

return 0; // renvoie 0

}

return 1; // renvoie 1

}

int decode_int(char c) // declaration de la fonction qui renvoie le nombre associ? au caractere

{

c=c-'0';

return c;

}

int decode_nombre(char * ch, int n) // fonction qui recupere les n premiers chiffres d'une chaine

{

int i;

int somme=0;

for (i=0;i<n;i++)

{

somme=(somme*10) + decode_int(ch[i]);

}

return somme;

}

float conversion_longitude(char * lon) // fonction qui convertie une longitude en nombre flottant

{

float result_convers,val_deci;

int val_ent_dec,val_ent_minut;

result_convers=decode_nombre(lon,3);// recuperation de la partie entiere

val_ent_minut=decode_nombre(&lon[3],2);

val_ent_dec=decode_nombre(&lon[6],4);

val_deci=(val_ent_minut+(0.0001*val_ent_dec))/60; // recuperation de la valeur decimale en degr?

result_convers=result_convers+val_deci;

return result_convers;

}

float conversion_latitude(char * lat) // fonction qui convertie une longitude en nombre flottant

{

// 3723.2475 equivalent a 37 degr? 23.2475 / 60 degr?

float result_convers,val_deci;

int val_ent_dec,val_ent_minut;

char minute[7];

char min[4];

int i;

for (i=0;i<7; i++) minute[i]=lat[2+i];

for (i=0;i<4; i++) min[i]=lat[5+i];

result_convers=decode_nombre(lat,2);// recuperation de la partie entiere 37

val_ent_minut=decode_nombre(minute,2); // 23

val_ent_dec=decode_nombre(min,4); // 2475

val_deci=(val_ent_minut+(val_ent_dec*0.0001))*0.0166667; // recuperation de la valeur decimale en degr?

result_convers+=val_deci;

return result_convers;

}

int decode_trame(char* trame,position * pos) // fonction qui rends les trames "GPGGA" et qui renvoie 1 et affichage de la latitude et la longitude en floattant

{

int i;

char ch[10];

if (trame_cmp(trame,"GPGGA"))

{

for (i=0;i<10;i++) ch[i]=trame[29+i];

pos->latitude=conversion_latitude(trame+17);

pos->longitude=conversion_longitude(ch);

return 1;

}

return 0;

}

float conver_radian(float x) // fonction qui convertit un nombre flottant en flottaant

{

return (x*M_PI)/180;

}

float calcul_distance(position pos1,position pos2) // fonction qui calcule la distance entre deux positions

{

float result= (40000/360) * sqrt(((pos2.latitude - pos1.latitude)*(pos2.latitude - pos1.latitude))+((pos2.longitude - pos1.longitude)*(pos2.longitude - pos1.longitude))) ;

return result;

}

float calcul_vitesse(position pos1,position pos2) // fonction qui calcule la distance entre deux positions

{

float vitesse;

vitesse=3600*(calcul_distance(pos1,pos2)*Rayon);

return vitesse;

}

int distance_a_la_plus_proche_zone(position p,zone r[],int nb_zones,float *d) // fonction qui calcule la distance la plus proche

{

int index=0;

int i;

*d=fabs(calcul_distance(p,r[0].rpos)); // initialisation du minimum

float t;

if (nb_zones>0)

{

for (i=1;i<nb_zones;i++)

{

t=fabs(calcul_distance(p,r[i].rpos));

if (t<*d)

{

*d=t;

index=i;

}

}

return index;

}

return -1;

}

void test_distance_plus_proche(void) //

{

position p;

p.latitude =45.75;

p.longitude=3.110060;

int num_zone;

float d ;

num_zone=distance_a_la_plus_proche_zone(p,zones,sizeof(zones),&d);

if (num_zone!=2 & fabs(d-44.85)>0.5)

{

printf(("erreur test distance la plus proche "));

exit(-1);

}

}

float fonction_generique(char* ch) // fonction qui convertit la latitudes ou la longitude en flottant

{

float result_convers;

if(ch[4]=='.')

{

result_convers=conversion_latitude(ch);

}

else if(ch[5]=='.')

{

result_convers=conversion_longitude(ch);

}

return result_convers;

}

//Fonction ? modifier !!!!!

void traitement(char * trame)

{

/*

static int cpt=0;

cpt++;

if (trame_cmp(trame,"GPGGA"))

{printf ("> %s\n",trame);}

*/

position p;

position p_init;

p_init.latitude=45.750000;

p_init.longitude= 3.110060;

float distance;

float vitesse;

float seuil=1000;

int index;

if (decode_trame(trame,&p))

{

printf ("> %s\n",trame);

printf("la latitude est %f \n la longitude est %f \n ",p.latitude,p.longitude);

vitesse=calcul_vitesse(p_init,p);

index=distance_a_la_plus_proche_zone(p,zones,sizeof(zones),&distance);

if ((distance<seuil) && (vitesse>zones[index].vitmax))

{

printf(" alarme on \n");

printf(" zone numero %d \n ",index);

printf("la vitesse est %f\n ",vitesse);

printf("la distance est %f \n ",distance);

}

else

printf("Alarme off \n ");

p_init = p;

}

}

void test_decode_trame(void)

{

if (decode_trame("$GPGGA,141914.00,4545.0000,N,00306.6036,E,1,05,3.4,499.3,M,,M,,*7D",&test_position)!=1)

{

printf("Erreur Test decode trame \n");

exit(-1);

}

}

void test_decode_int(void)

{

if (decode_int('0')!=0)

{

printf("Erreur test unitaire decode int\n");

exit(-1);

}

if (decode_int('1')!=1)

{

printf("Erreur test unitaire decode int\n");

exit(-1);

}

if (decode_int('2')!=2)

{

printf("Erreur test unitaire decode int\n");

exit(-1);

}