Projet

Général

Profil

« Précédent | Suivant » 

Révision 671

Ajouté par jcguifodjo il y a presque 4 ans

Creation de la table zones

Voir les différences:

branch/GUIFO/sp4a12/main.c
#include <strings.h>
#include "trame.h"
#include "math.h"
#define k 111120
#define k 111.120 //Constante a utiliser pour calculer la distance en utilisant la m?thode de Pythagore
#define N 200
#define rads 0.01745329252
#define t 1/6
#define h 1/60
#define rads 0.01745329252 //rads=pie/180 puisque le langage C utilise les angles en radians
//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};
typedef struct {
float latitude;
float longitude;
} Position ;
//Comparaison des trames pour savoir si la trame est du bon type
int trame_cmp(char* trame,char* type)
{
......
return d;
}
//Conversion d'un caract?re en decimal
int decode_int(char c)
{
......
}
return d;
}
//Renvoi la valeur decimal des n premiers caract?res entr?e en param?tre
int decode_nombre(char *c, int n)
{
......
}
return valeur;
}
//Conversion de la latitude et longitude en degree floatant
float conversion(char *c)
{
int i=0;
float deg_lat,deg_long;
float test;
while(c[i]!='\0'){
i++;
......
}
else{
d=0;
printf("\nNe peut decode cette trame");
}
printf("\nTrames: %s", trame);
......
printf("\nLongitude: %2.7f", (*p).longitude);
}
//Test pour d?coder la latitude et la longitude des trames "$GPGGA" en degr? d?cimaux
// calcule la racine carr?e en se basant sur l'algorithme de Heron
float racine_carree(float nb){
float a,fa,e;
e=1;
a=1;
while(e>0.0001){
fa=((nb/a)+a)/2;
if(fa>a)
{
e=fa-a;
}
else
e=a-fa;
a=fa;
}
return a;
}
//Fonction qui calcule la distance entre deux positions
float calcul_distance(Position p1,Position p2){
float x,a,y,z,distance,b;
......
b=(p1.latitude+p2.latitude)/2;
x=a*cos(b*rads);
y=p2.latitude-p1.latitude;
z=sqrt((x*x)+(y*y));
z=racine_carree((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 calcule_vitesse(Position p1, Position p2){
float distance,vitesse;
distance=calcul_distance(Position p1, Position p2);
distance=calcul_distance( p1, p2);
vitesse=3.6*distance;
return vitesse;*
return vitesse;
}*/
}
typedef struct {
Position rpos;
float vitmax;
} Zone ;
Zone zones[] = {
{{44.7887762, -3.012}, 50}, /* Descripteur de la premi?re zone */
{{44.7891220, -3,013}, 70},
};
//Calcul de la distance ? la zone dangereuse la plus proche
int distance_a_la_proche_zone(Position p, Zone r[],int nb_zones, float *d){
int i;
float dist;
if(nb_zones!=0){
(*d)=calcul_distance(p,r[0].rpos);
for(i=1;i<nb_zones;i++){
dist=calcul_distance(p,r[i].rpos);
if(dist<(*d)){
(*d)=dist;
}
}
}
return i;
}
//Fonction ? modifier !!!!!
void traitement(char * trame)
{
Position p;
int d;
static int cpt=0 ;
cpt++ ;
if (trame_cmp(trame,"GPGGA")==1)
branch/GUIFO/sp4a3/sp4a3_kalman.c
//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++)
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]);
}
}
double C[na][mb];
if(ma==nb){
for (i=0; i<na; i++){
for (j=0; j<mb; j++)
{
C[i][j]=0;
for (k=0; k<nb; k++){
C[i][j] += (A[i][k])*(B[k][j]);
}
out[i][j]=C[i][j];
}
}
}
}
void tests_unitaires(void){
//Matrices d'entrée
double T21a[2][1]={{7},{-5}};
......
}
int main(int argc,char **argv){
tests_unitaires();
......
debug=1; ///Mettre à 1 pour afficher les matrices.
///Ajouter votre code ci-dessous///
// Kalman
Plot_Mat(X," X(k|k) = ");
// X = F*X
Mul_Mat_Mat(4,4,F,4,1,X,X);
Plot_Mat(X," X(k+1|k) = ");
Plot_Mat(P,"P(k|k) = ");
//P = F*P*F'+Q;
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);
double FP[4][4],FPFT[4][4];
Mul_Mat_Mat(4,4,F,4,4,P,FP);
Mul_Mat_Mat(4,4,FP,4,4,FT,FPFT);
Add_Mat_Mat(4,4,FPFT,4,4,Q,P);
Plot_Mat(P,"P(k+1|k) = F.P(k|k).FT + Q = ");
// K = P*H' / ( H*P*H' + R);
Mul_Mat_Mat(4,4,P,4,2,HT,A2);
Mul_Mat_Mat(2,4,H,4,2,A2,B);
Add_Mat_Mat(2,2,B,2,2,R,C);
double HP[2][4],HPHT[2][2],D[2][2],C[2][2];
double PHT[4][2];
Mul_Mat_Mat(2,4,H,4,4,P,HP);
Mul_Mat_Mat(4,4,P,4,2,HT,PHT);
Plot_Mat(PHT,"P(k+1|k).HT = ");
Mul_Mat_Mat(2,4,HP,4,2,HT,HPHT);
Add_Mat_Mat(2,2,HPHT,2,2,R,C);
Plot_Mat(C,"H.P(k+1|k).HT+R = ");
Inverse_Mat_22(2,2,C,D) ;
Mul_Mat_Mat(4,2,A2,2,2,D,K);
Plot_Mat(D,"INV(H.P(k+1|k).HT+R) = ");
Mul_Mat_Mat(4,2,PHT,2,2,D,K);
Plot_Mat(K,"K = ");
//X = X + K*([xb(i);yb(i)]-H*X);
double delta[2][1],Obs[2][1],Kdelta[4][1],HX[2][1];
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);
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 = ");
Plot_Mat(Obs,"Obs = ");
Mul_Mat_Mat(2,4,H,4,1,X,HX);
Sub_Mat_Mat(2,1,Obs,2,1,HX,delta);
Plot_Mat(delta,"DELTA = Obs - H.X(k+1|k)");
Mul_Mat_Mat(4,2,K,2,1,delta,Kdelta);
Add_Mat_Mat(4,1,X,4,1,Kdelta,X);
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);
double KH[4][4],KHP[4][4];
Mul_Mat_Mat(4,2,K,2,4,H,KH);
Mul_Mat_Mat(4,4,KH,4,4,P,KHP);
Sub_Mat_Mat(4,4,P,4,4,KHP,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 ///
......
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 ++;
cpt ++;
}
fclose(Fout);
fclose(fichier);

Formats disponibles : Unified diff