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:

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