Révision 632
Ajouté par jcguifodjo il y a environ 4 ans
| branch/GUIFO/sp4a12/main.c | ||
|---|---|---|
|
#include <strings.h>
|
||
|
#include "trame.h"
|
||
|
#include "math.h"
|
||
|
#define k 0.017453292519943
|
||
|
#define k 111120
|
||
|
#define N 200
|
||
|
#define rads 0.01745329252
|
||
|
|
||
|
//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",
|
||
| ... | ... | |
|
"$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};
|
||
|
0};
|
||
|
|
||
|
typedef struct {
|
||
|
float latitude;
|
||
|
float longitude;
|
||
| ... | ... | |
|
for(i=0;i<n;i++){
|
||
|
valeur+=decode_int(c[i])*pow(10,n-1-i);
|
||
|
}
|
||
|
|
||
|
return valeur;
|
||
|
}
|
||
|
//Conversion de la latitude et longitude en degree floatant
|
||
|
float conversion(char *c)
|
||
| ... | ... | |
|
p.latitude=-p.latitude;
|
||
|
}
|
||
|
|
||
|
if(indx_lat[0]=='W'){
|
||
|
if(indx_lon[0]=='W'){
|
||
|
p.longitude=-p.longitude;
|
||
|
}
|
||
|
}
|
||
|
}
|
||
|
else{
|
||
|
d=0;
|
||
|
|
||
|
}
|
||
|
return d;
|
||
|
}
|
||
|
|
||
|
|
||
|
//Fonction qui calcule la distance entre deux positions
|
||
|
float calcul_distance(Position p1,Position p2){
|
||
|
float x,a,y,z,distance,b;
|
||
|
a=(p2.longitude-p1.longitude);
|
||
|
b=(p1.latitude+p2.latitude)/2;
|
||
|
x=a*cos(b*rads);
|
||
|
y=p2.latitude-p1.latitude;
|
||
|
z=sqrt((x*x)+(y*y));
|
||
|
distance=k*z;
|
||
|
return distance;
|
||
|
|
||
|
}
|
||
|
|
||
|
//Fonction qui calcule la vitesse du vehicule par seconde
|
||
|
/*float calcule_vitesse(Position p1, Position p2){
|
||
|
|
||
|
float distance,vitesse;
|
||
|
distance=calcul_distance(Position p1, Position p2);
|
||
|
vitesse=3.6*distance;
|
||
|
return vitesse;*
|
||
|
|
||
|
}*/
|
||
|
//Fonction ? modifier !!!!!
|
||
|
void traitement(char * trame)
|
||
|
{
|
||
|
int d;
|
||
|
static int cpt=0 ;
|
||
|
cpt++ ;
|
||
|
if (trame_cmp(trame,"GPGGA")==1)
|
||
|
{
|
||
|
printf ("> %s\n",trame);
|
||
|
d=decode_trame(trame,Position p);
|
||
|
printf ("d= %d",d);
|
||
|
|
||
|
}
|
||
|
|
||
|
}
|
||
| ... | ... | |
|
printf ("Erreur Test unitaire basique decode nombre.\n");
|
||
|
exit(-1);
|
||
|
}
|
||
|
if (decode_nombre("632",2)!=63){
|
||
|
if (decode_nombre("400",2)!=40){
|
||
|
printf ("Erreur Test unitaire basique decode nombre.\n");
|
||
|
exit(-1);
|
||
|
}
|
||
| branch/GUIFO/sp4a3/sp4a3_kalman.c | ||
|---|---|---|
|
Plot_Mat(X," X(k+1|k) = ");
|
||
|
|
||
|
//P = F*P*F'+Q;
|
||
|
double A0[4][4],A1[4][4],A2[4][2],A3[2][4],A4[2][4],B[2][2],D[2][2],C[2][2],E[2][2];
|
||
|
double delta[2][1],Obs[2][1];
|
||
|
double A0[4][4],A1[4][4],A2[4][2],A3[4][4],A4[4][4],B[2][2],D[2][2],C[2][2],E[2][1];
|
||
|
double delta[2][1],Obs[2][1],C1[4][1];
|
||
|
Mul_Mat_Mat(4,4,F,4,4,P,A0);
|
||
|
Mul_Mat_Mat(4,4,A0,4,4,FT,A1);
|
||
|
Add_Mat_Mat(4,4,A1,4,4,Q,P);
|
||
| ... | ... | |
|
Mul_Mat_Mat(2,4,H,4,2,A2,B);
|
||
|
Add_Mat_Mat(2,2,B,2,2,R,C);
|
||
|
Inverse_Mat_22(2,2,C,D) ;
|
||
|
Mul_Mat_Mat(2,2,A2,2,2,D,K);
|
||
|
Mul_Mat_Mat(4,2,A2,2,2,D,K);
|
||
|
Plot_Mat(K,"K = ");
|
||
|
|
||
|
//X = X + K*([xb(i);yb(i)]-H*X);
|
||
|
|
||
|
//Plot_Mat(Delta,"DELTA = Obs - H.X(k+1|k)");
|
||
|
|
||
|
|
||
|
Mul_Mat_Mat(2,4,H,4,1,X,E);
|
||
|
Sub_Mat_Mat(2,1,Obs,2,1,E,delta);
|
||
|
Mul_Mat_Mat(4,2,K,2,1,delta,C1);
|
||
|
Add_Mat_Mat(4,1,X,4,1,C1,X);
|
||
|
Plot_Mat(X," X(k+1|k+1) = X(k+1|k) + K.Delta = ");
|
||
|
|
||
|
// P = P - K*H*P;
|
||
|
Mul_Mat_Mat(2,2,K,2,4,H,A3);
|
||
|
Mul_Mat_Mat(2,4,A3,4,4,P,A4);
|
||
|
Sub_Mat_Mat(4,4,P,2,4,A4,P);
|
||
|
Mul_Mat_Mat(4,2,K,2,4,H,A3);
|
||
|
Mul_Mat_Mat(4,4,A3,4,4,P,A4);
|
||
|
Sub_Mat_Mat(4,4,P,4,4,A4,P);
|
||
|
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 ///
|
||
Formats disponibles : Unified diff
Implémentation de la fonction calcul vitesse
Et implémentation de la matrice observation du filtre de Kalman