Projet

Général

Profil

« Précédent | Suivant » 

Révision 282

Ajouté par celasherme il y a presque 4 ans

Fonction Kalman a finir et revoir la fonction convlog du tp2

Voir les différences:

branch/lashermes/sp4a12/main.c
"$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};
int trame_cmp(char* trame,char* type)
typedef struct // definition de la structure
{
float latitude;
float longitude;
} Position;
typedef struct // permet de definir une zone
{
Position rpos;
float vitmax;
}Zone;
Zone zones[] = {{{44.208904, -1.297033},50}, {{47.478419, -0.563},70}, {{46.401488, 6.59049},50}, {{48.856614, 2.3522219},90}};
int nb_Zones = 4;
int trame_cmp(char* trame,char* type) // permet de verifier qu'on a la trame "GPGGA"
{
int i=0;
do
{
......
return 1;
}
int decode_int(char c)
int decode_int(char c) // permet de decoder en decimal
{
int carac;
unsigned int carac;
carac=c-48;
if(carac<0 || carac>9)
{
......
return carac;
}
//Fonction ? modifier !!!!!
void traitement(char * trame) // permet de reconaitre les trame GPGGA
{
static int cpt=0;
Position pos;
cpt++;
if (trame_cmp(trame,"GPGGA")==1)
{
printf ("%s\nLatitude: %f; Longitude: %f\n\n", trame, pos.latitude, pos.longitude);
}
}
// Fonction convertissant la chaine de carac en float
int decode_nombre(char * ch, int n)
{
......
return entier;
}
// Fonction pour convertir latitude en float
// Fonction pour convertir latitude en float !!!!! Ne fonctionne pas a revoir
float convlat (char * latitude)
{
float carac=0;
......
}
int decode_trame(char *trame , *p )
int decode_trame(char *trame , Position *p )
{
if (trame_cmp(trame,"GPGGA")); // si la bonne trame
if (trame_cmp(trame,"GPGGA")) // si la bonne trame
{
int pos_virgule=0;
int i=0;
int j=0;
int k=0;
char Lat[10];
char Long[12];
while (trame[i] != '\0')
{
if(pos_virgule==2 && i !=26)
{
Lat[k]=trame[i];
k++;
}
if(pos_virgule==4 && i !=39)
{
Long[j]=trame[i];
j++;
}
if(trame[i]==',')
{
pos_virgule +=1;
}
i++;
}
p->latitude=convlat(Lat);
p->longitude=convlog(Long);
return 1;
}
else
{
return 0;
}
}
//A refaire
/*float calcule_distance(Position p_1, Position p_2)
{
float rpt = 0.01745329251;
float distance;
float delta =p_1.longitude-p_2.longitude;
float dist = 6378.1*acos(sin(p_1.latitude*rpt)*sin(p_2.latitude*rpt)+ cos(p_1.latitude*rpt)*cos(p_2.latitude*rpt)*cos(delta*rpt));
return dist;
}
//Fonction ? modifier !!!!!
void traitement(char * trame) // permet de reconaitre les trame GPGGA
float calcule_vitesse(Position p_1, Position p_2)
{
static int cpt=0;
cpt++;
if (trame_cmp(trame,"GPGGA")==1)
float vitesse;
float dist = calcule_distance(p_1,p_2);
vitesse = dist*3600;
}*/
int distance_a_la_plus_proche_zone(Position p, Zone r[], int nb_zones, float *d)
{
if (nb_zones ==0)
{
printf ("> %s\n",trame);
}
return -1;
}
else
{
int i;
int curseur =0;
float distance_min = 10000;
float distance;
for (i=0;i<nb_zones; i++)
{
distance =calcule_distance(p, r[i].rpos);
if (distance <= distance_min)
{
distance_min = distance;
curseur = i;
}
}
*d = distance_min;
return curseur;
}
}
//Ajouter vos tests unitaires dans cette fonction.
......
if (5!=5){
printf ("Erreur Test unitaire basique.\n");
exit(-1);
}
}
//Tests unitaires de trame_cmp
if (trame_cmp("$GPGGA suite chaine","GPGGA")!=1){
printf ("Erreur Test unitaire trame_cmp.\n");
exit(-1);
......
printf ("Erreur Test unitaire decode_int\n");
exit(-1);
}
//Tests unitaires de conversion
if (decode_nombre("12",2)!=12)
{
printf ("Erreur Test unitaire decode_nombre\n");
......
printf ("Erreur Test unitaire decode_nombre\n");
exit(-1);
}
//Test unitaires de decode_trame
Position test;
if (decode_trame("$GPGSV,3,3,10,22,39,053,50,28,15,320,*7E", &test) != 0){
printf("Erreur test unitaire decode_trame.\n");
exit(-1);
}
if (decode_trame("$GPGGA,141914.00,4545.0000,N,00306.6036,E,1,05,3.4,499.3,M,,M,,*7D", &test)!=1){
printf("Erreur test unitaire decode_trame.\n");
exit(-1);
}
if (decode_trame("$GPGGA,141925.00,4545.2410,N,00306.6046,E,1,05,3.4,501.4,M,,M,,*7D", &test)!=1){
printf("Erreur test unitaire decode_trame.\n");
exit(-1);
}
if (decode_trame("$GPRMC,141920.00,A,4545.6419,N,00306.6039,E,0.2,133.1,010206,,*38", &test) != 0){
printf("Erreur test unitaire decode_trame.\n");
exit(-1);
}
//Tests unitaires de distance a la plus proche zone
//Tests unitaires de conversion Latitude et Longitude
if (convlat("0045.0000")-(float) 0.75>0.0001)
{
printf("Erreur Test unitaire convlat\n");
exit(-1);
}
if (convlog("00306.6036")- (float)3.11006>0.0001)
{
printf("Erreur Test unitaire convlog.\n");
exit(-1);
}
if (convlog("00008.0015")-(float)0.133358>0.0001)
{
printf("Erreur Test unitaire convlog\n");
exit(-1);
}
}
branch/lashermes/sp4a3/sp4a3_kalman.c
#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]){
void Add_Mat_Mat(int na,int ma,double A[na][ma],int nb,int mb,double b[nb][mb], double out[na][ma])
{
if(na==nb && ma==mb )
{
int i,j;
for(i=0;i<=na-1;i++)
{
for(j=0;j<=ma-1;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]){
void Inverse_Mat_22(int n,int m,double A[n][m],double B[n][m])
{
if (m==2 && n==2)
{
double det=A[0][0]*A[1][1]-A[0][1]*A[1][0];
if(det!=0)
{
B[0][0]= (double) A[1][1]/det;
B[0][1]= (double) -A[0][1]/det;
B[1][0]= (double) -A[1][0]/det;
B[1][1]= (double) A[0][0]/det;
}
}
}
void Transpose_Mat(int n,int m,double A[n][m],double R[m][n]){
......
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]){
void Sub_Mat_Mat(int na,int ma,double A[na][ma],int nb,int mb,double b[nb][mb], double out[na][ma]){
if(na==nb && ma==mb ){
int i,j;
for(i=0;i<=na-1;i++){
for(j=0;j<=ma-1;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]){
void Mul_Mat_Mat(int na,int ma,double A[na][ma], int nb,int mb,double B[nb][mb], double out[na][mb])
{
if(ma==nb){
int i,j,k;
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];
}
}
}
}
}
......
{0, 0, 1, 0},
{0, 0, 0, 1}};
double FT[4][4];
Transpose_Mat(4,4,F,FT);
Transpose_Mat(4,4,F,FT);
double C[2][2];
double CT[2][2];
Transpose_Mat(2,2,C,CT);
while(fscanf(fichier, "%lf %lf %lf", &t, &x, &y)>0){
printf("-------------%04d--------------\n",cpt);
......
{
t -= t0;x -= x0;y -= y0;
debug=0; ///Mettre à 1 pour afficher les matrices.
debug=1; ///Mettre à 1 pour afficher les matrices.
///Ajouter votre code ci-dessous///
// Kalman
// Kalman
// X = F*X
Plot_Mat(X," X(k+1|k) = ");
// X = F*X
double A[4][1];
Mul_Mat_Mat(4,4,F,4,1,X,A);
Plot_Mat(A," X(k+1|k) = ");
//P = F*P*F'+Q;
Plot_Mat(P,"P(k+1|k) = F.P(k|k).FT + Q = ");
//P = F*P*F'+Q;
double B[4][4];
double D[4][4];
Mul_Mat_Mat(4,4,F,4,4,P,B);
Mul_Mat_Mat(4,4,B,4,4,FT,D);
Add_Mat_Mat(4,4,D,4,4,Q,P);
Plot_Mat(P,"P(k+1|k) = F.P(k|k).FT + Q = ");
// K = P*H' / ( H*P*H' + R);
// K = P*H' / ( H*P*H' + R);
double L[4][2];
double C[2][2];
double E[2][4];
double G[2][2];
Mul_Mat_Mat(4,4,P,4,2,HT,L);
Mul_Mat_Mat(2,4,H,4,4,P,E);
Mul_Mat_Mat(2,4,E,4,2,HT,G);
Add_Mat_Mat(2,2,G,2,2,R,C);
Add_Mat_Mat(4,4,L,2,2,CT,K);
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 = ");
//Plot_Mat(Delta,"DELTA = Obs - H.X(k+1|k)");
double N[2][1];
double O[4][1];
double posi[2][1]={x,y};
double I[2][1];
double XC[4][1];
X[4][1]=XC[4][1];
Mul_Mat_Mat(2,4,H,4,1,X,I);
Sub_Mat_Mat(2,1,I,2,1,posi,N);
Mul_Mat_Mat(4,2,K,2,1,N,O);
Add_Mat_Mat(4,1,X,4,1,O,XC);
Plot_Mat(XC," 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) = ");
// P = P - K*H*P;
double M[4][4];
double J[4][4];
double PC[4][4];
P[4][4]=PC[4][4];
Mul_Mat_Mat(4,2,K,2,4,H,J);
Mul_Mat_Mat(4,4,J,4,4,P,M);
Sub_Mat_Mat(4,4,M,4,4,P,PC);
Plot_Mat(PC," 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