Projet

Général

Profil

« Précédent | Suivant » 

Révision 576

Ajouté par Domingos Joao BRAVO il y a presque 3 ans

Fin du TP3. Le programme fonctionne bien comme prévu.

Voir les différences:

sp4a3_kalman.c
{
for(j=0;j<mb;j++)
{
AddM=A[i][j] + B[i][j];
AddM=A[i][j] + B[i][j]; //On somme les élements qui se trouvent dans les mêmes positions
R[i][j]=AddM;
}
}
......
{
double Det;
Det=(A[0][0]*A[1][1])-(A[0][1]*A[1][0]);//Calcul du déterminant
R[0][0]=(1/Det)*A[1][1];
Det=(A[0][0]*A[1][1])-(A[0][1]*A[1][0]);//Calcul du déterminant: Det(A={(a,b);(c,d)}=(ad-cb)
//(A)^(-1)=(1/det(A))*{(d,-b),(-c,a)}
R[0][0]=(1/Det)*A[1][1];//La matrice résultat a le même nombre de ligne que la matrice A
R[0][1]=-(1/Det)*A[0][1];
R[1][0]=-(1/Det)*A[1][0];
R[1][1]=(1/Det)*A[0][0];
......
{
for(j=0;j<mb;j++)
{
SubM=A[i][j] - B[i][j];
SubM=A[i][j] - B[i][j];//On soustrait les élements qui se trouvent dans les mêmes positions
R[i][j]=SubM;
}
}
......
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)
if(ma==nb)//Si le nombre de colonnes de la matrice A égale au nombre de ligne de la matrice B
{
for (int i = 0; i < na; i++)
for (i = 0; i < na; i++)//La matrice résultat a le même nombre de ligne que la matrice A
{
for (int j = 0; j < mb; j++)
for (j = 0; j < mb; j++)//La matrice résultat a le même nombre de colonne que la matrice B
{
R[i][j] = 0;
for (int k = 0; k < ma; k++)
for (k = 0; k < ma; k++)
{
R[i][j] += A[i][k] * B[k][j];
R[i][j] += A[i][k] * B[k][j];//On somme les résultats de la multiplication des élements de chaque ligne de A par celles de chaque colonne de B
}
}
}
......
double FT[4][4];
Transpose_Mat(4,4,F,FT);
double X_l[4][1],X_2[2][1],X_3[2][1],X_4[4][1],X_5[4][1];
double P_1[4][4],P_2[4][4],P_3[4][4],P4[4][4],P5[4][4],P6[4][4];
double X_1[4][1],X_2[2][1],X_3[2][1],X_4[4][1],X_5[4][1];
double P_1[4][4],P_2[4][4],P_3[4][4],P_4[4][4],P_5[4][4],P_6[4][4];
double K_1[4][2],K_2[2][2],K_3[2][2],K_4[2][2],K_5[4][2],K_6[4][2];
......
t -= t0;xobs -= x0;yobs -= y0;
debug=1; ///Mettre à 1 pour afficher les matrices.
///Ajouter votre code ci-dessous///
///Ajouter votre code ci-dessous///
// Kalman
double Mobservation[2][1] ={{xobs},{yobs}};
// X = F*X
Mul_Mat_Mat(4,4,F, 4,1,X, X_1); //Multiplication de F et X, résultat stocké dans X_1
Plot_Mat(X_1," X(k+1|k) = ");
// X = F*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,P_1);//P_1=F*P
Mul_Mat_Mat(4,4,P_1, 4,4, FT,P_2);//P_2=P_1*FT=
Add_Mat_Mat(4,4,P_2,4,4,Q, P_3);//P_3=P_2+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_3,K_1);//K_1=H*P_3
Mul_Mat_Mat(2,4,K_1, 4,2, HT,K_2);//K_2=K1*HT
Add_Mat_Mat(2,2,K_2,2,2,R, K_3);//K_3=K_2+R
Inverse_Mat_22(2,2,K_3,K_4);//K_4=(K_3)^(-1)
Mul_Mat_Mat(4,4,P_3, 4,2, HT,K_5);//K_5=P_3*HT
Mul_Mat_Mat(4,2,K_5, 2,2, K_4,K_6);//K_6=K_5*K_4
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(X," X(k+1|k+1) = X(k+1|k) + K.Delta = ");
//X = X + K*([xobs(i);yobs(i)]-H*X);
Mul_Mat_Mat(2,4,H, 4,1, X_1,X_2);//X_2=H*X_1
Sub_Mat_Mat(2,1,Mobservation,2,1,X_2,X_3);//X_3=Mobservation-X_2
//Plot_Mat(Delta,"DELTA = Obs - H.X(k+1|k)");
Plot_Mat(X_3,"DELTA = Obs - H.X(k+1|k)");
Mul_Mat_Mat(4,2,K_6, 2,1,X_3,X_4);//X_4=K_6*X_3
Add_Mat_Mat(4,1,X_1,4,1,X_4,X_5);//X_5=X_1+X_4
// P = P - K*H*P;
Plot_Mat(P," P(k+1|k+1) = P(k+1|k) - K.H.P(k+1|k) = ");
Plot_Mat(X_5," X(k+1|k+1) = X(k+1|k) + K.Delta = ");
/// La matrice X doit contenir la position filtrée ///
// P = P - K*H*P;
Mul_Mat_Mat(4,2,K_6, 2,4, H,P_4);//P_4=K_6*H
Mul_Mat_Mat(4,4,P_4, 4,4, P_3,P_5);//P_5=P_4*P_3
Sub_Mat_Mat(4,4,P_3,4,4,P_5, P_6);//P_6=P_3-P_5
Plot_Mat(P_6," P(k+1|k+1) = P(k+1|k) - K.H.P(k+1|k) = ");
/// La matrice X doit contenir la position filtrée ///
int i, j;
for ( i = 0; i <4 ; i++)
{
for ( j = 0; j <1 ; j++)
{
X[i][j] = X_5[i][j];
}
}
for ( i = 0; i <4 ; i++)
{
for ( j = 0; j <4 ; j++)
{
P[i][j] = P_6[i][j];
}
}
}
t = cpt * dt;
dx = (xobs - oldx)/dt;

Formats disponibles : Unified diff