Cuby56

#pragma once
#include "stdafx.h" 

class KalmanFilter { 

	private:
        static bool init;
	static bool m_bMeasurement; 
	//---------------------------------------------------------
        static CvKalman* m_pKalmanFilter; 
        static CvRandState rng; 
	//---------------------------------------------------------
        static CvMat* state;
        static CvMat* process_noise; 
        static CvMat* measurement; 
        static CvMat* measurement_noise; 

	public: 
        KalmanFilter(void); 
        ~KalmanFilter(void); 
	//---------------------------------------------------------
	static void initialize(); 
        static bool isInitialized();
        static void predictionBegin(float x, float y); 
        static void predictionUpdate(float x, float y); 
        static void predictionReport(CvPoint & pnt); 
}; 

#include "stdafx.h"
#include "Form1.h"
#include "KalmanFilter.h" 

//=================================================================
//=== Defination ===
//=================================================================

bool KalmanFilter::init;
bool KalmanFilter::m_bMeasurement;
//---------------------------------------------------------
CvKalman* KalmanFilter::m_pKalmanFilter; 
CvRandState KalmanFilter::rng; 
//---------------------------------------------------------
CvMat* KalmanFilter::state; 
CvMat* KalmanFilter::process_noise; 
CvMat* KalmanFilter::measurement; 
CvMat* KalmanFilter::measurement_noise; 

//=================================================================
//=== Constructor / Destructor ===
//=================================================================

KalmanFilter::KalmanFilter() { } 

KalmanFilter::~KalmanFilter() { 

    cvReleaseMat( & state ); 
    cvReleaseMat( & process_noise ); 
    cvReleaseMat( & measurement ); 
    cvReleaseMat( & measurement_noise ); 
    cvReleaseKalman( & m_pKalmanFilter ); 
} 

//=================================================================
//=== Initialized ===
//=================================================================

bool KalmanFilter::isInitialized() { return init; }

void KalmanFilter::initialize() { 

    init = false; 
    int dynam_params = 4;	// x,y,dx,dy
    int measure_params = 2; 
    m_pKalmanFilter = cvCreateKalman(dynam_params, measure_params); 
    state = cvCreateMat( dynam_params, 1, CV_32FC1 ); 
//---------------------------------------------------------
    cvRandInit( &rng, 0, 1, -1, CV_RAND_UNI ); 
    cvRandSetRange( & rng, 0, 1, 0 ); 
    rng.disttype = CV_RAND_NORMAL; 
    cvRand( & rng, state ); 
//---------------------------------------------------------
    process_noise = cvCreateMat( dynam_params, 1, CV_32FC1 );
    measurement = cvCreateMat( measure_params, 1, CV_32FC1 );
    measurement_noise = cvCreateMat( measure_params, 1, CV_32FC1 );
    cvZero(measurement); 
//---------------------------------------------------------
    const float F[] = { 
            1, 0, 0, 0,	//x
            0, 1, 0, 0,	//y 
            0, 0, 1, 0,	//dx = dx 
            0, 0, 0, 1,	//dy = dy 
    }; 
//---------------------------------------------------------
    memcpy( m_pKalmanFilter->transition_matrix->data.fl, F, sizeof(F)); 
    cvSetIdentity( m_pKalmanFilter->measurement_matrix, cvRealScalar(1) );			// (H) 
    cvSetIdentity( m_pKalmanFilter->process_noise_cov, cvRealScalar(1e-4) );		// (Q) 1e-5
    cvSetIdentity( m_pKalmanFilter->measurement_noise_cov, cvRealScalar(1e-1) );	// (R) 
    cvSetIdentity( m_pKalmanFilter->error_cov_post, cvRealScalar(0.1));				// 1
//---------------------------------------------------------
    cvRand( & rng, m_pKalmanFilter->state_post ); 
} 

//=================================================================
//=== Begin ===
//=================================================================

void KalmanFilter::predictionBegin(float x , float y) {

    init = true; 
    m_pKalmanFilter->state_post->data.fl[0] = x;
    m_pKalmanFilter->state_post->data.fl[1] = y;
    m_pKalmanFilter->state_post->data.fl[2] = (float)0;
    m_pKalmanFilter->state_post->data.fl[3] = (float)0;
} 

//=================================================================
//=== Update ===
//=================================================================

void KalmanFilter::predictionUpdate(float x, float y) { 

    state->data.fl[0] = x;
    state->data.fl[1] = y;
    m_bMeasurement = true; 
} 

//=================================================================
//=== Report ===
//=================================================================

void KalmanFilter::predictionReport(CvPoint &pnt) {

    const CvMat* prediction = cvKalmanPredict( m_pKalmanFilter, 0 ); 
    pnt.x = prediction->data.fl[0]; 
    pnt.y = prediction->data.fl[1]; 
//---------------------------------------------------------
    if (m_bMeasurement)
        m_bMeasurement = false; 
    else
    { 
        state->data.fl[0] = pnt.x; 
        state->data.fl[1] = pnt.y; 
    } 
//---------------------------------------------------------
    cvRandSetRange( & rng, 0, sqrt(m_pKalmanFilter->measurement_noise_cov->data.fl[0]), 0 ); 
    cvRand( & rng, measurement_noise ); 
//---------------------------------------------------------
    cvMatMulAdd( m_pKalmanFilter->measurement_matrix, state, measurement_noise, measurement ); 
//---------------------------------------------------------
    cvKalmanCorrect( m_pKalmanFilter, measurement ); 
//---------------------------------------------------------
    cvRandSetRange( &rng, 0, sqrt(m_pKalmanFilter->process_noise_cov->data.fl[0]), 0 ); 
    cvRand( &rng, process_noise ); 
//---------------------------------------------------------
    cvMatMulAdd( m_pKalmanFilter->transition_matrix, state, process_noise, state ); 
} 

 
KalmanFilter::initialize();			// initial
//---------------------------------------------------------

KalmanFilter::predictionBegin( x0 , y0 );	// first
//---------------------------------------------------------

KalmanFilter::predictionUpdate( xn , yn );	// now
KalmanFilter::predictionReport( new_p );	// new