Projet

Général

Profil

« Précédent | Suivant » 

Révision 651

Ajouté par jcguifodjo il y a presque 4 ans

Correction decode_trame et conversion de latitude et longitude en dégrée flottant
Fin de l'implémentation du filtre de Kalman

Voir les différences:

branch/GUIFO/sp4a12/main.c
#define k 111120
#define N 200
#define rads 0.01745329252
#define t 1/6
#define h 1/60
//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",
......
//Conversion de la latitude et longitude en degree floatant
float conversion(char *c)
{
int i=0,j;
int i=0;
float deg_lat,deg_long;
float test;
while(c[i]!='\0'){
i++;
}
if(i==8){
if(i==9){
deg_lat=decode_nombre(c,2);
deg_lat+=(c[2]-'0')*1/6;
deg_lat+=(c[3]-'0')*1/60;
deg_lat+=(c[2]-'0')*0.16666666666666;
deg_lat+=(c[3]-'0')*0.016666666666666;
for(i=5;i<9;i++){
deg_lat+=((c[i]-'0')*pow(10,3-i))/6;
}
return deg_lat;
}
if(i==9){
if(i==10){
deg_long=decode_nombre(c,3);
deg_long+=(c[3]-'0')*1/6;
deg_long+=(c[4]-'0')*1/60;
deg_long+=(c[3]-'0')*0.166666666666666;
deg_long+=(c[4]-'0')*0.0166666666666666;
for(i=6;i<10;i++){
deg_long+=((c[i]-'0')*pow(10,4-i))/6;
deg_long+=((c[i]-'0')*pow(10,4-i))*0.166666666666666;
}
return deg_long;
}
......
void decode_trame(char *trame, Position* p)
{
int i,j,d;
char lat[N],lon[N],indx_lat[]={'0','\0'},indx_lon[]={'0','\0'};
char lat[]={'0','0','0','0','0','0','0','0','0','\0'},lon[]={'0','0','0','0','0','0','0','0','0','0','\0'},indx_lat[]={'0','\0'},indx_lon[]={'0','\0'};
if(trame_cmp(trame,"GPGGA")==1){
d=1;
for (i=0; i<8; i++){
for (i=0; i<9; i++){
lat[i]=trame[i+17];
}
for (j=0; j<9;j++){
lon[i]=trame[i+29];
for (j=0; j<10;j++){
lon[j]=trame[j+29];
}
indx_lat[0]=trame[27];
indx_lon[0]=trame[40];
......
}
printf("\nTrames: %s", trame);
printf("\nLatitude: %2.2f", (*p).latitude);
printf("\nLongitude: %2.2f", (*p).longitude);
printf("\nLatitude: %2.7f", (*p).latitude);
printf("\nLongitude: %2.7f", (*p).longitude);
}
//Test pour d?coder la latitude et la longitude des trames "$GPGGA" en degr? d?cimaux
//Fonction qui calcule la distance entre deux positions
float calcul_distance(Position p1,Position p2){
......
cpt++ ;
if (trame_cmp(trame,"GPGGA")==1)
{
decode_trame(trame, &p);
decode_trame(trame,&p);
}
branch/GUIFO/sp4a3/sp4a3_kalman.c
#include "sp4a3_kalman_extra.h"
//Addition de matrices
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;
......
}
//Inversion d'une matrice
void Inverse_Mat_22(int n,int m,double A[n][m],double B[n][m]){
double det;
......
B[1][0]=-(A[1][0])/det;
B[1][1]=(A[0][0])/det;
}
//Transposée de matrices
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];
}
//Soustraction de matrices
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;
for(i=0;i<na;i++)
for(j=0;j<ma;j++)
out[i][j]=A[i][j]-b[i][j];
}
//Multiplication de matrices
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,j,k;
for (i=0; i<na; i++)
......
Plot_Mat(K,"K = ");
//X = X + K*([xb(i);yb(i)]-H*X);
//Plot_Mat(Delta,"DELTA = Obs - H.X(k+1|k)");
Obs[0][0]=x;
Obs[1][0]=y;
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);
Add_Mat_Mat(4,1,X,4,1,C1,X);
Plot_Mat(Obs,"Obs = ");
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;
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);

Formats disponibles : Unified diff