kalmanFilter.cpp

Go to the documentation of this file.

#include "kalmanFilter.h"

KalmanFilter::KalmanFilter()
{
        int params = 4; // x,y,dx,dy
        int measured = 2; //x,y
        myKalmanFilter = cvCreateKalman(params, measured);
        measurement_Z = cvCreateMat( measured, 1, CV_32FC1 );

        // Transition matrix
        const float F[] = {
                                        1, 0, 1, 0,
                                        0, 1, 0, 1,
                                        0, 0, 1, 0,
                                        0, 0, 0, 1,
        };
        memcpy( myKalmanFilter->transition_matrix->data.fl, F, sizeof(F));                                      // (F)
        cvSetIdentity( myKalmanFilter->measurement_matrix, cvRealScalar(1) );                           // (H)
        cvSetIdentity( myKalmanFilter->process_noise_cov, cvRealScalar(1e-5) );                 // (Q)
        cvSetIdentity( myKalmanFilter->measurement_noise_cov, cvRealScalar(1e-1) ); // (R)
        //Covariant matrix initialized to zero means we know exactly initial position, non
        //zero should be a value near to estimated initial value variations
        cvSetIdentity( myKalmanFilter->error_cov_post, cvRealScalar(1));

        InitializeCriticalSection(&mutexPrediction);
}

KalmanFilter::~KalmanFilter() {
        // Destroy matrices
        cvReleaseMat( &measurement_Z );
        cvReleaseMat( &measurementNoise_V );
        cvReleaseKalman( &myKalmanFilter );

        DeleteCriticalSection(&mutexPrediction);
}



void KalmanFilter::predictionBegin(float x, float y) {
        //Initial corrected State
        myKalmanFilter->state_post->data.fl[0] = x;
        myKalmanFilter->state_post->data.fl[1] = y;
        myKalmanFilter->state_post->data.fl[2] = (float)0; //dx
        myKalmanFilter->state_post->data.fl[3] = (float)0; //dy 
        //Initial measure 
        predictionUpdate(x,y);
}

void KalmanFilter::predictionUpdate(float x, float y) {
        EnterCriticalSection(&mutexPrediction);
        measurement_Z->data.fl[0] = x;
        measurement_Z->data.fl[1] = y;
        bMeasurement = true;
        LeaveCriticalSection(&mutexPrediction);
}

void KalmanFilter::predictionGet(CvPoint &pnt) {
        EnterCriticalSection(&mutexPrediction);
        // Kalman prediction 
        const CvMat* prediction = cvKalmanPredict( myKalmanFilter, 0 );
        pnt.x = prediction->data.fl[0];
        pnt.y = prediction->data.fl[1];
        
        if (bMeasurement){ // update with the user position
                bMeasurement = false;
        } else { // update with the predicted position 
                measurement_Z->data.fl[0] = prediction->data.fl[0];
                measurement_Z->data.fl[1] = prediction->data.fl[1];
        }
        
        // adjust Kalman filter state
        cvKalmanCorrect( myKalmanFilter, measurement_Z );
        LeaveCriticalSection(&mutexPrediction);
} 

---