Projet

Général

Profil

« Précédent | Suivant » 

Révision 280

Ajouté par abseck1 il y a presque 4 ans

avancement sur le filtre de Kalman

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]){
int i,j=0;
if(na==nb && ma==mb)
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]){
float Idet;
Idet=1/((A[0][0]*A[1][1])-(A[0][1]*A[1][0]));
B[0][0]=Idet*A[1][1];
B[1][1]=Idet*A[0][0];
B[0][1]=-Idet*(A[0][1]);
B[1][0]=-Idet*(A[1][0]);
}
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]){
int i,j=0;
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 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]){
int i=0;
int j=0;
int k=0;
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 tests_unitaires(void){
//Matrices d'entrée
double T21a[2][1]={{7},{-5}};
......
double RAT44[4][4]={{13,15,21,19},{27,27,30,48},{87,53,19,9},{59,14,15,49}};
double RST44[4][4]={{-11,-11,-7,-11},{-15,-17,-16,-32},{-69,-37,-5,3},{-49,-6,-9,-45}};
double RMT44T44[4][4]={{816,412,192,304},{1155,583,379,687},{1146,668,466,758},{486,308,222,338}};
Plot_Mat(DELTA,"DELTA = Obs - H.X(k+1|k) ");
printf("Execution des tests unitaires.\n");
Transpose_Mat(2,4,T24,R42); if (!Equal_Mat_Mat(RTT24,R42)) error("Erreur calcul Transposition 2x4");
Transpose_Mat(4,4,T44a,R44); if (!Equal_Mat_Mat(RTT44,R44)) error("Erreur calcul Transposition 4x4");
......
// Kalman
// X = F*X
Plot_Mat(X," X(k+1|k) = ");
Plot_Mat(X," X(k+1|k) = ");
Mul_Mat_Mat(4,4,F,4,1,Xp,X); // Xp=F*X
Plot_Mat(X," X(k+1|k) = ");
Plot_Mat(P,"P(k|k) = ");
double A[4][4];
double A1[4][4];
Mul_Mat_Mat(4,4,F,4,4,P,A); // F*P = A
Mul_Mat_Mat(4,4,A,4,4,FT,A1); // F*P*FT=A1
Add_Mat_Mat(4,4,A1,4,4,Q,P); //P = F*P*FT+Q;
//P = F*P*F'+Q;
Plot_Mat(P,"P(k+1|k) = F.P(k|k).FT + Q = ");
Plot_Mat(P,"P(k+1|k) = F.P(k|k).FT + Q = ");
double B[2][4];
double INVERS[2][2];
double B1[2][2];
double B2[2][2];
Mul_Mat_Mat(2,4,H,4,4,P,B); // HP=B
Mul_Mat_Mat(2,4,B,4,2,HT,B1); // H*P*HT = B1
Add_Mat_Mat(2,2,B1,2,2,R,B2); // H*P*HT + R = B2
Plot_Mat(B2,"H.P(k+1|k).HT+R = ");
Inverse_Mat_22(2,2,B2,INVERS); // 1/(H*P*HT + R) = B2
Plot_Mat(INVERS,"INV(H.P(k+1|k).HT+R) = ");
double D[4][2];
Mul_Mat_Mat(4,4,P,4,2,HT,D);
Mul_Mat_Mat(4,2,D,2,2,INVERS,K); // P*HT/(H*P*HT + R) = K
// K = P*H' / ( H*P*H' + R);
Plot_Mat(K,"K = ");
Plot_Mat(K,"K = ");
double DELTA1[2][1];
double DELTA[2][1];
double W[2][1];
double F[4][1];
Mul_Mat_Mat(2,4,H,4,1,X,W);
Sub_Mat_Mat(2,1,obs,2,1,W,DELTA1);
Mul_Mat_Mat(4,2,K,2,1,DELTA1,F);
X=X + K*([xobs(i);yobs(i)]-H*X);
X=X + K*DELTA
Sub_Mat_Mat(2,1,obs,2,1,W,DELTA);
Plot Mat(obs,"obs=")
Plot_Mat(DELTA,"DELTA = Obs - H.X(k+1|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(X," X(k+1|k+1) = X(k+1|k) + K.Delta = ");
double G1[4][4];
double G[4][4];
// P = P - K*H*P;
Plot_Mat(P," P(k+1|k+1) = P(k+1|k) - K.H.P(k+1|k) = ");

Formats disponibles : Unified diff