Projet

Général

Profil

« Précédent | Suivant » 

Révision 277

Ajouté par jgsoleilla il y a presque 4 ans

TP3 SP4: Implementation filtre de Kalman

Voir les différences:

branch/soleillant/sp4a3/sp4a3_kalman.c
#include "sp4a3_kalman_extra.h"
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];
}
}
}
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]){
int i,j;
if (na == nb && ma == mb){
for (i=0;i<na;i++){
for (j=0;j<ma;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]){
if (A[0][0] != 0 && A[1][1] != 0 && n == 2 && n == 2){
float coef = 1/(A[0][0]*A[1][1] - A[0][1]*A[1][0]);
B[0][0] = coef * A[1][1];
B[0][1] = -(coef * A[0][1]);
B[1][0] = -( coef * A[1][0]);
B[1][1] = coef * A[0][0];
}
}
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];
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;
if ((na == nb)&&(ma == mb)){
for (i=0;i<na;i++){
for (j=0;j<ma;j++){
out[i][j]=A[i][j]-B[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 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;
if (ma == nb){
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];
}
}
}
}
}
void Cop_Mat(int n,int m,double A[n][m], double out[n][m]){
int i,j;
for(i=0;i<n;i++){
for (j=0;j<m;j++){
out[i][j] = A[i][j];
}
}
}
void tests_unitaires(void){
//Matrices d'entrée
double T21a[2][1]={{7},{-5}};
......
{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 Obs[2][1] = {{0},
{0}};
double Delta[2][1] = {{0},
{0}};
double tampon[4][4] = {{0, 0, 0, 0},
{0, 0, 0, 0},
{0, 0, 0, 0},
{0, 0, 0, 0}};
double tampon2[4][4] = {{0, 0, 0, 0},
{0, 0, 0, 0},
{0, 0, 0, 0},
{0, 0, 0, 0}};
while(fscanf(fichier, "%lf %lf %lf", &t, &x, &y)>0){
printf("-------------%04d--------------\n",cpt);
......
///Ajouter votre code ci-dessous///
// Kalman
// X = F*X
// X = F*X
Mul_Mat_Mat(4,4,F,4,1,X,tampon);
Cop_Mat(4,1,tampon,X);
Plot_Mat(X," X(k+1|k) = ");
//P = F*P*F'+Q;
//P = F*P*F'+Q;
Mul_Mat_Mat(4,4,F,4,4,P,tampon); //P=F*P
Mul_Mat_Mat(4,4,tampon,4,4,FT,tampon2); //P=P*F'
Add_Mat_Mat(4,4,tampon2,4,4,Q,P);//P=P+Q
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(2,4,H,4,4,P,tampon);//K=H*P
Mul_Mat_Mat(2,4,tampon,4,2,HT,K);//K=K*H'
Add_Mat_Mat(2,2,K,2,2,R,tampon);//K=K+R
Inverse_Mat_22(2,2,tampon,K);//K=K^-1
Mul_Mat_Mat(4,2,HT,2,2,K,tampon);//K=H'*K
Mul_Mat_Mat(4,4,P,4,2,tampon,K);//K=P*K
Plot_Mat(K,"K = ");
//X = X + K*([xb(i);yb(i)]-H*X);
//Plot_Mat(Delta,"DELTA = Obs - H.X(k+1|k)");
//Obs = [2][1] x y
Obs[0][0] = x;
Obs[1][0] = y;
//Delta = Obs - H.X
Mul_Mat_Mat(2,4,H,4,1,X,tampon);//H*X
Sub_Mat_Mat(2,1,Obs,2,1,tampon,Delta);//Obs-tampon
Plot_Mat(Delta,"DELTA = Obs - H.X(k+1|k)");
//X = X + K*([xb(i);yb(i)]-H*X);
Mul_Mat_Mat(4,2,K,2,1,Delta,tampon);//K*Delta
Add_Mat_Mat(4,1,X,4,1,tampon,tampon2);//X+tampon
Cop_Mat(4,1,tampon2,X);
Plot_Mat(X," X(k+1|k+1) = X(k+1|k) + K.Delta = ");
// P = P - K*H*P;
// P = P - K*H*P;
Mul_Mat_Mat(4,2,K,2,4,H,tampon);//tampon = K*H
Mul_Mat_Mat(4,4,tampon,4,4,P,tampon2);//tampon2 = tampon*P
Sub_Mat_Mat(4,4,P,4,4,tampon2,tampon);//P = P-tampon2
Cop_Mat(4,4,tampon,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