OpenCVKalman.h

Go to the documentation of this file.

// -*- mode: c++; indent-tabs-mode: nil; c-basic-offset: 4; coding: iso-8859-1; -*-

/*
ACT-CV - Machine Vision for Cognitive Modeling
Copyright (c) 2008 Marc Halbruegge

This program is free software; you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation; either version 2 of the License, or
(at your option) any later version.

This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
GNU General Public License for more details.

You should have received a copy of the GNU General Public License
along with this program; if not, write to the Free Software
Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
*/


#ifndef OPENCVKALMAN_H_
#define OPENCVKALMAN_H_

#include <time.h>

template<unsigned int _dims>
class KalmanFilter {
    CvKalman* kalman;
    CvMat* measurement;
   
    // for setting to random state
    CvRandState rng;
public:
    KalmanFilter() {
        kalman = cvCreateKalman(2*_dims, _dims, 0 );
        measurement = cvCreateMat(_dims, 1, CV_32FC1 );
        
        time_t seed;
        time(&seed);
        cvRandInit(&rng, (double) 0, (double) 0, (int)seed);
    }
    ~KalmanFilter() {
        cvReleaseKalman(&kalman);
        cvReleaseMat(&measurement);
    }
 
    void InitRandom(float m, float sd, float processNoise, float measurementNoise) {

        // transition matrix
        float A[2*_dims][2*_dims];
        for (int i=0;i<2*_dims;i++) {
            for (int j=0;j<2*_dims;j++) {
                A[i][j]=0;
            }
        }
        // pos + vel stay as they are
        for (int i=0;i<2*_dims;i++) {
            A[i][i]=1;
        }
        // add vel to pos
        for (int i=0;i<_dims;i++) {
            A[i][i+_dims]=1;
        }
        
        memcpy(kalman->transition_matrix->data.fl, A, sizeof(A));

        cvSetIdentity(kalman->measurement_matrix, cvRealScalar(1) );
        cvSetIdentity(kalman->process_noise_cov, cvRealScalar(processNoise) );
        cvSetIdentity(kalman->measurement_noise_cov, cvRealScalar(measurementNoise) );

        //cvSetIdentity(kalman->error_cov_post, cvRealScalar(1));
        cvSetIdentity(kalman->error_cov_post, cvRealScalar(sd));
        cvSetIdentity(kalman->error_cov_pre, cvRealScalar(sd));

        /* choose random initial state */
#if 0
        cvRandSetRange(&rng, sd, m, -1);
        rng.disttype = CV_RAND_NORMAL;
        cvRand(&rng, kalman->state_post);
#else
        for (int i=0;i<_dims;i++) {
            kalman->state_post->data.fl[i]=m; // position
            kalman->state_post->data.fl[_dims+i]=0; // velocity
        }
#endif                
    }
    
    void AddMeasure(const float *x) {
        memcpy(measurement->data.fl, x, sizeof(float)*_dims);
        cvKalmanCorrect(kalman, measurement);
    }
    
    void GetCorrectedState(float *x) {
        memcpy(x, kalman->state_post->data.fl, sizeof(float)*_dims*2);
    }
    
    void GetVariance(float *x) {
        for (int i=0;i<_dims*2;i++) {
            x[i]=cvGetReal2D(kalman->error_cov_post, i, i);
        }        
    }

    const CvMat* Predict() {
        return cvKalmanPredict(kalman, 0);
    }
    
    void Predict(float *x) {
        const CvMat *prediction = Predict();
        memcpy(x, prediction->data.fl, sizeof(float)*_dims*2);
    }
};


#endif /*OPENCVKALMAN_H_*/