Kalman filters - Part 2

In a previous post I described kalman filters and gave a very simple example of a 1D filter implemented in C. In the real world, having a filter with three or more inputs is common. A standard 3rd order example would be a filter that considers the position, velocity and acceleration of a system.

In this case you would have a 3x3 measurement matrix (H) (with the diagonal set to what you can measure), a 3x3 process noise matrix (Q), a 3x3 measurement noise matrix (R), and a 3x3 state transition matrix. The standard transition matrix would simply contain v+=a*dt, x+=v*dt, but we can use a 2nd order leapfrog integrator to get better accuracy of our position from our acceleration data. So in total we have:

You can either solve this using the standard kalman matrix maths, but you will find that an SVD approach will give you more stable solutions. OpenCV is one library that already has the kalman matrix maths built in. To set up a kalman filter with OpenCV we just need to setup the matrices we need and call the appropriate cvKalman functions.

(ugly) Code snippet follows:

#ifndef KALMAN_H
#define KALMAN_H

/** OpenCV Kalman filter for 1st, 2nd and 3rd order position, velocity and acceleration data.
    (c) Adrian Boeing
    www.adrianboeing.com
 */
#include <cv.h>

typedef enum {
    K_P = 1, //position only
    K_V = 2, //velocity only
    K_A = 4, //acceleration only
    K_PV = 3, //position & velocity
    K_PA = 5, //position & acceleration
    K_VA = 6, //velocity & acceleration
    K_PVA = 7 //position, velocity and acceleration
} Kalman_e;

class KalmanPVA {
public:
    KalmanPVA(int order, Kalman_e type, float initial_pos, float initial_vel, float initial_accel, float dt, float process_noise[3], float measurement_noise[3]) {
        m_order = order;
        //create the OpenCV kalman structures
        kalman = cvCreateKalman(order, 3, 0 );
        z = cvCreateMat(3,1,CV_32FC1);
        
        //construct the transition matrix
        SetDT(dt);
        
        //enable only the measurements we have!
        for (int i=0;i<m_order;i++) {
            cvmSet(kalman->measurement_matrix,i,i,((type&7)>>i)&1);
        }
        //configure the process noise matrix
        cvSetIdentity( kalman->process_noise_cov,    cvRealScalar(1) ); //zero the matrix 
        for (int i=0;i<m_order;i++) {
            cvmSet( kalman->process_noise_cov,i,i, process_noise[i] ); 
        }
        //configure the measurment noise matrix
        cvSetIdentity( kalman->measurement_noise_cov, cvRealScalar(1) ); //zero the matrix
        for (int i=0;i<3;i++) {
            cvmSet(kalman->measurement_noise_cov,i,i,measurement_noise[i]);
        }
        cvSetIdentity( kalman->error_cov_post,cvRealScalar(10) ); //large initial unknown
        
        //setup the initial state
        cvmSet(kalman->state_post,0,0,initial_pos );
        if (m_order>1)
            cvmSet(kalman->state_post,1,0,initial_vel);
        if (m_order>2)
            cvmSet(kalman->state_post,2,0,initial_accel);
        
        
    };
    void SetDT(float dt) {
        //3rd, 2nd, and 1st order transition matrix models
        
        //3rd order uses 2nd order leapfrog integration
        float F3[] = {   1, dt, 0.5*dt*dt,
            0,  1, dt,
            0,  0,  1};
        
        float F2[] = {   1, dt, 
            0,  1};
        
        float F1[] = {1};
        
        switch (m_order) {
            case 3:
                memcpy( kalman->transition_matrix->data.fl, F3, sizeof(F3));
                break;
            case 2:
                memcpy( kalman->transition_matrix->data.fl, F2, sizeof(F2));
                break;
            case 1:
                memcpy( kalman->transition_matrix->data.fl, F1, sizeof(F1));
                break;
        }
    }
    void SetMeasurements(float pos, float vel, float accel) {
        cvmSet(z,0,0,pos);
        //if (z->rows>1)
            cvmSet(z,1,0,vel);
        //if (z->rows>2)
            cvmSet(z,2,0,accel);
    }
    
    void Update(){
        const CvMat* y = cvKalmanPredict( kalman, 0 );
        cvKalmanCorrect( kalman, z );
    }
    float GetPositionEstimate() {
        float v = cvmGet(kalman->state_post,0,0);
        return v;
    }
    float GetVelocityEstimate() {
        if (kalman->state_post->rows>1)
            return cvmGet(kalman->state_post,1,0);
        return 0;
    }
    float GetAccelerationEstimate() {
        if (kalman->state_post->rows>2)
            return cvmGet(kalman->state_post,2,0);
        return 0;
    }
    float GetPositionError() {
        return cvmGet(kalman->error_cov_post,0,0);
    }
    CvKalman* kalman; //openCV Kalman filter
    CvMat* z; //measurement matrix
private:

    void cvKalmanNoObservation() { 
        cvCopy(kalman->error_cov_pre, kalman->error_cov_post); 
        cvCopy(kalman->state_pre, kalman->state_post); 
    } 
    int m_order;
    
};

#endif