Projet

Général

Profil

« Précédent | Suivant » 

Révision 547

Ajouté par Clara SOULARD il y a presque 3 ans

mise en place du filtre de kalman, résultats validés.

Voir les différences:

tag/rc_1/clara_soulard/sp4a3/sp4a3_kalman.c
#include <stdio.h>
#include <math.h>
#include "sp4a3_kalman_extra.h"
#include "sp4a3_kalman_extra.h"
void init (int na,int ma,double A[na][ma])
{
int i, j;
for(i=0; i<na; i++)
{
for(j=0; j<ma; j++)
{
A[i][j]=0;
}
}
}
void Add_Mat_Mat(int na,int ma,double A[na][ma],int nb,int mb,double B[nb][mb], double R[na][ma]){
void Add_Mat_Mat(int na,int ma,double A[na][ma],int nb,int mb,double B[nb][mb], double R[na][ma]){
init(na, ma, R);
int i,j;
for (i=0; i<na; i++)
{
for (j=0; j<ma; j++)
{
R[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]){
double det;
det=1/(A[0][0]*A[1][1]-A[0][1]*A[1][0]);
if (det==0)
{
printf("Erreur d'inversion, le determinant de la matrice est nul");
exit (-1);
}
B[0][0]=A[1][1]*det;
B[1][1]=A[0][0]*det;
B[0][1]=-A[0][1]*det;
B[1][0]=-A[1][0]*det;
}
......
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 R[na][ma]){
void Sub_Mat_Mat(int na,int ma,double A[na][ma],int nb,int mb,double B[nb][mb], double R[na][ma]){
int i,j;
for (i=0; i<na; i++)
{
for (j=0; j<ma; j++)
{
R[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 R[na][mb]){
void Mul_Mat_Mat(int na,int ma,double A[na][ma], int nb,int mb,double B[nb][mb], double R[na][mb]){
int i, j, k;
if (ma!=nb)
{
printf("Taille des matrices non conformes");
exit (-1);
}
for(i=0; i<na; i++)
{
for(j=0; j<mb; j++)
{
R[i][j]=0;
for(k=0; k<ma; k++)
{
R[i][j]+=A[i][k]*B[k][j];
}
}
}
}
......
// kalman param
double sigma_etat = 10.0;
double sigma_observation = 2.0;
double X[4][1] = {{0},{0},{0},{0}};
double X[4][1] = {{0},{0},{0},{0}};
double Xrep[4][1] = {{0},{0},{0},{0}};
double X2[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}};
{0, 0, 0, 0}};
double Prep[4][4] = {{0, 0, 0, 0},
{0, 0, 0, 0},
{0, 0, 0, 0},
{0, 0, 0, 0}};
double P2[4][4] = {{0, 0, 0, 0},
{0, 0, 0, 0},
{0, 0, 0, 0},
{0, 0, 0, 0}};
double FP[4][4] = {{0, 0, 0, 0},
{0, 0, 0, 0},
{0, 0, 0, 0},
{0, 0, 0, 0}};
double FPFt[4][4] = {{0, 0, 0, 0},
{0, 0, 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},
......
double R[2][2] = {{sigma_observation*sigma_observation, 0},
{0 , sigma_observation*sigma_observation}};
double K[4][2];
double K[4][2];
double Krep[4][2];
double H[2][4] = {{1, 0, 0, 0},
{0, 1, 0, 0}};
double HT[4][2];
......
{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 kPHT[4][4];
init(4, 4, kPHT);
double kHP[2][4];
init(2, 4, kHP);
double kHPHT[2][2];
init(2, 2, kHPHT);
double kHPHTR[2][2];
init(2, 2, kHPHT);
double invkHPHTR[2][2];
init(2, 2, invkHPHTR);
double delta[2][1];
init(2, 1, delta);
double HX[2][1];
init(2, 1, HX);
double Kdelta[4][1];
init(4, 1, Kdelta);
double KH[4][4];
init(4,4, KH);
double KHP[4][4];
init(4,4, KHP);
double zero[4][1];
init(4, 1, zero);
double zero2[4][4];
init(4, 4, zero);
while(fscanf(fichier, "%lf %lf %lf", &t, &xobs, &yobs)>0){
printf("-------------%04d--------------\n",cpt);
......
t0=t;x0=xobs;y0=yobs;
xobs=xobs-x0;yobs=yobs-y0;
Plot_Mat(F,"F = ");
Plot_Mat(H,"H = ");
Plot_Mat(H,"H = "); //4x2*2*3 donne une 4*3
Plot_Mat(R,"R = ");
}
else
......
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;
// X = F*X
Mul_Mat_Mat(4,4,F, 4,1, X, Xrep);
Add_Mat_Mat(4, 1, Xrep, 4, 1, zero, X);
Plot_Mat(X," X(k+1|k) = ");
//P = F*P*F'+Q;
Mul_Mat_Mat(4, 4, F, 4, 4, P, FP);
Plot_Mat(P, "P kk");
Mul_Mat_Mat(4, 4, FP, 4, 4, FT, FPFt);
Plot_Mat(FPFt, "avant additon avec Q");
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);
// K = P*H' / ( H*P*H' + R);
Mul_Mat_Mat(4,4,P, 4, 2, HT, kPHT); //taille ok
Plot_Mat(kPHT, "PHT");
Mul_Mat_Mat(2,4, H, 4,4, P, kHP); //taille ok
Mul_Mat_Mat(2,4, kHP, 4, 2, HT, kHPHT); //taille ok
Plot_Mat(kHPHT, "matrice hpht");
Add_Mat_Mat(2,2, kHPHT, 2, 2, R, kHPHTR); //taille ok;
Plot_Mat(kHPHTR, "matrice avant inverse");
Inverse_Mat_22(2,2, kHPHTR, invkHPHTR); //taille ok;
Plot_Mat(invkHPHTR, "inv matrice = ");
Mul_Mat_Mat(4, 2, kPHT, 2, 2, invkHPHTR, K); //taille ok
Plot_Mat(K,"K = ");
//X = X + K*([xobs(i);yobs(i)]-H*X);
//Plot_Mat(Delta,"DELTA = Obs - H.X(k+1|k)");
//Plot_Mat(delta,"DELTA = Obs - H.X(k+1|k)");
Mul_Mat_Mat(2, 4, H, 4, 1, X, HX);
delta[0][0]=xobs-HX[0][0];
delta[1][0]=yobs-HX[1][0];
Plot_Mat(delta,"DELTA = Obs - H.X(k+1|k)");
//printf("%f, %f", delta[0][0], delta[1][0]);
Mul_Mat_Mat(4, 2, K, 2, 1, delta, Kdelta);
Add_Mat_Mat(4, 1, X, 4, 1, Kdelta, Xrep);
Add_Mat_Mat(4, 1, Xrep, 4, 1, zero, X);
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) = ");
// P = P - K*H*P;
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, Prep);
Add_Mat_Mat(4, 4, Prep, 4, 4, zero2, 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