Implémentation du filtre Kalman - ce qui pourrait être faux

Question

Je suis désolé d'être si ennuyeux mais j'ai révisé mon code plusieurs fois avec l'aide d'une douzaine d'articles mais mon KF ne fonctionne toujours pas. Par "ne fonctionne pas", je veux dire que les estimations de KF sont fausses. Voici un bon paste de positions réelles, bruitées et KF estimées (juste un petit morceau).

Mon exemple est le même que dans tous les didacticiels que j'ai trouvés - J'ai un vecteur d'état de position et de vélocité. La position est en mètres et représente la position verticale dans l'air. Mon cas réel est le parachutisme (avec parachute). Dans mon échantillon de données générées, j'ai supposé que nous commençons à 3000m et la vitesse est de 10m/s.

P.S .: Je suis sûr que les calculs matriciels sont OK - il doit y avoir une erreur avec la logique.

Ici je produis des données:

void generateData(float** inData, float** noisedData, int x, int y){ 
    inData[0][0]= 3000; //start position 
    inData[1][0]= -10; // 10m/s velocity; minus because we assume it's falling 

    noisedData[0][0]= 2998; 
    noisedData[1][0]= -10; 

    for(int i=1; i<x; i++){ 
     inData[0][i]= inData[0][i-1] + inData[1][i-1]; 
     inData[1][i]= inData[1][i-1]; //the velocity doesn't change for simplicity's sake 

     noisedData[0][i]=inData[0][i]+(rand()%6-3); //we add noise to real measurement 
     noisedData[1][i]=inData[1][i]; //velocity has no noise 
    } 
} 

Et ceci est mon implémentation (initialisation des matrices est basée sur Wikipedia Kalman example):

int main(int argc, char** argv) { 
    srand(time(NULL)); 

    float** inData = createMatrix(100,2); //2 rows, 100 columns 
    float** noisedData = createMatrix(100,2); 
    float** estData = createMatrix(100,2); 

    generateData(inData, noisedData, 100, 2); 

    float sampleRate=0.1; //10hz 

    float** A=createMatrix(2,2); 
    A[0][0]=1; 
    A[0][1]=sampleRate; 
    A[1][0]=0; 
    A[1][1]=1; 

    float** B=createMatrix(1,2); 
    B[0][0]=pow(sampleRate,2)/2; 
    B[1][0]=sampleRate; 

    float** C=createMatrix(2,1); 
    C[0][0]=1; //we measure only position 
    C[0][1]=0; 


    float u=1.0; //acceleration magnitude 
    float accel_noise=0.2; //acceleration noise 
    float measure_noise=1.5; //1.5 m standard deviation 
    float R=pow(measure_noise,2); //measure covariance 
    float** Q=createMatrix(2,2); //process covariance 
    Q[0][0]=pow(accel_noise,2)*(pow(sampleRate,4)/4); 
    Q[0][1]=pow(accel_noise,2)*(pow(sampleRate,3)/2); 
    Q[1][0]=pow(accel_noise,2)*(pow(sampleRate,3)/2); 
    Q[1][1]=pow(accel_noise,2)*pow(sampleRate,2); 

    float** P=createMatrix(2,2); //covariance update 
    P[0][0]=0; 
    P[0][1]=0; 
    P[1][0]=0; 
    P[1][1]=0; 

    float** P_est=createMatrix(2,2); 
    P_est[0][0]=P[0][0]; 
    P_est[0][1]=P[0][1]; 
    P_est[1][0]=P[1][0]; 
    P_est[1][1]=P[1][1]; 

    float** K=createMatrix(1,2); //Kalman gain 

    float** X_est=createMatrix(1,2); //our estimated state 
    X_est[0][0]=3000; X_est[1][0]=10; 

    // !! KALMAN ALGORITHM START !! // 
    for(int i=0; i<100; i++) 
    {   
     float** temp; 
     float** temp2; 
     float** temp3; 

     float** C_trans=matrixTranspose(C,2,1); 
     temp=matrixMultiply(P_est,C_trans,2,2,1,2); //2x1 
     temp2=matrixMultiply(C,P_est,2,1,2,2); //1x2 
     temp3=matrixMultiply(temp2,C_trans,2,1,1,2); //1x1 
     temp3[0][0]+=R; 
     K[0][0]=temp[0][0]/temp3[0][0]; // 1. KALMAN GAIN 
     K[1][0]=temp[1][0]/temp3[0][0]; 

     temp=matrixMultiply(C,X_est,2,1,1,2); 
     float diff=noisedData[0][i]-temp[0][0]; //diff between meas and est 

     X_est[0][0]=X_est[0][0]+(K[0][0]*diff); // 2. ESTIMATION CORRECTION 
     X_est[1][0]=X_est[1][0]+(K[1][0]*diff); 

     temp=createMatrix(2,2); 
     temp[0][0]=1; temp[0][1]=0; temp[1][0]=0; temp[1][1]=1; 
     temp2=matrixMultiply(K,C,1,2,2,1); 
     temp3=matrixSub(temp,temp2,2,2,2,2); 
     P=matrixMultiply(temp3,P_est,2,2,2,2); // 3. COVARIANCE UPDATE 



     temp=matrixMultiply(A,X_est,2,2,1,2); 
     X_est[0][0]=temp[0][0]+B[0][0]*u; 
     X_est[1][0]=temp[1][0]+B[1][0]*u; // 4. PREDICT NEXT STATE 


     temp=matrixMultiply(A,P,2,2,2,2); 
     float** A_inv=getInverse(A,2); 
     temp2=matrixMultiply(temp,A_inv,2,2,2,2); 
     P_est=matrixAdd(temp2,Q,2,2,2,2); // 5. PREDICT NEXT COVARIANCE 


     estData[0][i]=X_est[0][0]; //just saving here for later to write out 
     estData[1][i]=X_est[1][0]; 
    } 

    for(int i=0; i<100; i++) printf("%4.2f : %4.2f : %4.2f \n", inData[0][i], noisedData[0][i], estData[0][i]); // just writing out 

    return (EXIT_SUCCESS); 
} 

Answer 1

Il semble que vous supposiez un modèle de corps rigide pour le problème. Si tel est le cas, alors pour le problème que vous êtes en train de résoudre, je ne mettrais pas l'entrée lorsque vous ferez la mise à jour du processus pour prédire l'état suivant. Peut-être qu'il me manque quelque chose mais l'entrée u ne joue aucun rôle dans la génération des données. Permettez-moi de le formuler autrement, en définissant u sur +1, votre modèle suppose que le corps doit se déplacer dans la direction + x parce qu'il y a une entrée dans cette direction, mais la mesure lui indique de passer à autre manière. Donc, si vous mettez beaucoup de poids sur les mesures, cela va aller dans la direction de -ve, mais si vous mettez beaucoup de poids sur le modèle, il devrait aller dans la direction + ve. Quoi qu'il en soit, d'après les données générées, je ne vois pas de raison de mettre quoi que ce soit à part zéro.

Autre chose, votre taux d'échantillonnage est de 0,1 Hz. Mais lorsque vous générez des données, vous supposez qu'il s'agit d'une seconde, puisque chaque échantillon, la position est modifiée de -10 mètres par seconde.

Voici une implémentation matlab/octave.

l = 1000; 
Ts = 0.1; 
y = 3000; %measurement to be fed to KF 
v = -10; % METERS PER SECOND 
t = [y(1);v]; % truth for checking if its working 

for i=2:l 
    y(i) = y(i-1) + (v)*Ts; 
    t(:,i) = [y(i);v];   % copy to truth vector 
    y(i) = y(i) + randn;  % noise it up 
end 


%%%%% Let the filtering begin! 

% Define dynamics 
A = [1, Ts; 0, 1]; 
B = [0;0]; 
C = [1,0]; 

% Steady State Kalman Gain computed for R = 0.1, Q = [0,0;0,0.1] 
K = [0.44166;0.79889]; 

x_est_post = [3000;0]; 

for i=2:l 
    x_est_pre = A*x_est_post(:,i-1); % Process update! That is our estimate in case no measurement comes in. 

    %%% OMG A MEASUREMENT! 
    x_est_post(:,i) = x_est_pre + K*(-x_est_pre(1)+y(i)); 
end 

Answer 2

Vous faites beaucoup d'indexation de tableau bizarre.

float** A=createMatrix(2,2); 
A[0][0]=1; 
A[0][3]=sampleRate; 
A[1][0]=0; 
A[1][4]=1; 

Quel est le résultat attendu de l'indexation en dehors des limites du tableau?