Projet

Général

Profil

« Précédent | Suivant » 

Révision 458

Ajouté par Lilian MEMBRE il y a presque 3 ans

Filtre de kalman ok, les courbes sont juste

Voir les différences:

branch/MEMBRE/sp4a3/sp4a3_kalman.c
{
for (int j = 0; j < mb; j++)
{
R[i][ j]=A[i][j] + B[i][j];
R[i][ j]=A[i][j] + B[i][j]; /*Il additione termes à termes */
}
}
}
......
void Inverse_Mat_22(int n,int m,double A[n][m],double R[n][m])
{
int i,j;
int determinant = A[0][0]*A[1][1] - A[1][0]*A[0][1];
int determinant = A[0][0]*A[1][1] - A[1][0]*A[0][1]; /* calcul du determinant d'une matrice 2x2 */
R[0][0]=A[1][1]/determinant;
R[0][1]= - A[0][1]/determinant;
R[1][0]= - A[1][0]/determinant;
R[1][1]=A[0][0]/determinant;
R[1][1]=A[0][0]/determinant; /* Utilisation de la formule d'inversion des matrices 2x2 pour calculer R,matrice inverse de A */
}
......
{
for (j=0;j<m;j++)
{
R[j][i]=A[i][j];
R[j][i]=A[i][j]; /*transforme les lignes en colonnes */
}
}
}
......
{
for (int j = 0; j < mb; j++)
{
R[i][j] = A[i][j] - B[i][j];
R[i][j] = A[i][j] - B[i][j]; /* Soustrait termes à termes */
}
}
}
......
R[i][j] = 0;
for (int l = 0; l < ma; l++)
{
R[i][j] += A[i][l] * B[l][j];
R[i][j] += A[i][l] * B[l][j]; /* R(i,j)=Sommes(A(i,l)*B(l,j) */
}
}
}
......
{0, 0, 0, 1}};
double FT[4][4];
Transpose_Mat(4,4,F,FT);
double R1[4][4];
double R2[4][4];
double R3[4][4];
double R4[4][2];
double R5[2][4];
double R6[2][2];
double R7[2][2];
double R8[4][2];
double R9[2][2];
double R10[4][4];
double R11[4][4];
double R15[4][1];
double R16[2][1];
double X2[4][1];
double P2[4][4];
double Delta[2][1];
double X1 [4][1];
int i,j;
while(fscanf(fichier, "%lf %lf %lf", &t, &xobs, &yobs)>0){
printf("-------------%04d--------------\n",cpt);
......
///Ajouter votre code ci-dessous///
// Kalman
double R1[4][4];
double R2[4][4];
double R3[4][4];
double R4[4][2];
double R5[2][4];
double R6[2][2];
double R7[2][2];
double R8[4][2];
double R9[2][2];
double R10[4][4];
double R11[4][4];
double Matriceobs[2][1]={{xobs},
{yobs}};
// X = F*X
Mul_Mat_Mat(4, 4, F, 4, 1, X, X);
Plot_Mat(X," X(k+1|k) = ");
Mul_Mat_Mat(4, 4, F, 4, 1, X, X1);
Plot_Mat(X1," X(k+1|k) = ");
//P = F*P*F'+Q;
Mul_Mat_Mat(4,4,F, 4,4,P, R1);
Transpose_Mat(4, 4, F, R2);
Mul_Mat_Mat(4, 4, R1, 4, 4, R2, R3);
Add_Mat_Mat(4, 4, Q, 4, 4, R3, P);
Plot_Mat(P,"P(k+1|k) = F.P(k|k).FT + Q = ");
// K = P*H' / ( H*P*H' + R);
Transpose_Mat(2, 4, H, R4);
......
Inverse_Mat_22(2,2,R7, R9);
Mul_Mat_Mat(4,2,R8, 2,2, R9, K);
Plot_Mat(K,"K = ");
//DELTA = Obs - H.X(k+1|k)
Mul_Mat_Mat(2,4,H,4,1,X1,R16);
Sub_Mat_Mat(2,1,Matriceobs, 2,1,R16,Delta);
Plot_Mat(Delta,"DELTA = Obs - H.X(k+1|k)");
//X2 = X + K*([xobs(i);yobs(i)]-H*X); revient à faire X+K*Delta
Mul_Mat_Mat(4,2,K,2,1,Delta,R15);
Add_Mat_Mat(4,1,X1,4,1,R15,X2);
Plot_Mat(X2," X(k+1|k+1) = X(k+1|k) + K.Delta = ");
//X = X + K*([xobs(i);yobs(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 = ");
// P = P - K*H*P;
// P2 = P - K1*H*P;
Mul_Mat_Mat(4,2,K,2,4,H,R10);
Mul_Mat_Mat(4,4,R10,4,4,P,R11);
Sub_Mat_Mat(4,4,P,4,4,R11,P);
Plot_Mat(P," P(k+1|k+1) = P(k+1|k) - K.H.P(k+1|k) = ");
Sub_Mat_Mat(4,4,P,4,4,R11,P2);
Plot_Mat(P2," P(k+1|k+1) = P(k+1|k) - K.H.P(k+1|k) = ");
/*Mettre alors que P=P2 et X=X2 */
for (i = 0; i < 4; i++)
{
for (j = 0; j < 1; j++)
{
X[i][j]=X2[i][j];
}
}
for (i = 0; i < 4; i++)
{
for (j = 0; j < 4; j++)
{
P[i][j]=P2[i][j];
}
}
/// La matrice X doit contenir la position filtrée ///
}

Formats disponibles : Unified diff