Kalman Filter implementation - what could be wrong

Question

I am sorry for being this tedious but I reviewed my code several times with the help of a dozen of articles but still my KF doesn't work. By "doesn't work" I mean that the estimates by KF are wrong. Here is a nice paste of Real, Noised and KF estimated positions (just a small chunk).

My example is the same as in every tutorial I've found - I have a state vector of position and velocity. Position is in meters and represents vertical position in air. My real world case is skydiving (with parachute). In my sample generated data I've assumed we start at 3000m and the velocity is 10m/s.

P.S.: I am pretty sure matrix computations are OK - there must be an error with the logic.

Here I generate data:

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
    }  
}

And this is my implementation (matrices initialization is based on 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

It looks like you are assuming a rigid body model for the problem. If that is the case, then for the problem you are solving, I would not put in the input u when you do the process update to predict the next state. Maybe I am missing something but the input u does not play any role in generating the data.

Let me put it another way, setting u to +1 looks like your model is assuming that the body should move in the +x direction because there is an input in that direction, but the measurement is telling it to go the other way. So if you put a lot of weight on the measurements, it's going to go in the -ve direction, but if you put a lot of weight on the model, it should go in the +ve direction. Anyway, based on the data generated, I don't see a reason for setting u to anything but zero.

Another thing, your sampling rate is 0.1 Hz, But when you generate data, you are assuming it's one second, since every sample, the position is changed by -10 meters per second.

Here is a matlab/octave implementation.

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

You are doing a lot of weird array indexing.

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

What is the expected outcome of indexing outside of the bounds of the array?