kalman_sample.cpp

kalman filter-sample .. good luck

#include "kalman_filter.h"
#include <iostream>
using namespace std;

namespace egps
{
	namespace filter
	{
		
		kalman_filter::kalman_filter ()
		{
			meas = 0;
			lastLat = 0;
			lastLong = 0;
			
			const int MP = 4;
			const int DP = 4;
			// model transition matrix
			const float A[] =
			{
					// x*1 + 0*y + 1*dx + 0*dy = x
					1,    0,    1,     0,
					// x*0 + 1*y + 0*dx + 1*dy = y
					0,    1,    0,     1,
					// x*0 + 0*y + 1*dx + 0*dy = dx
					0,    0,    1,     0,
					// x*0 + 0*y + 0*dx + 1*dy = dy 
					0,    0,    0,     1
			};
			
			// measurement transition matrix
			const float H[] =
			{
				1,0,0,0,
				0,1,0,0,
				0,0,1,0,
				0,0,0,1
			};
			
			output.dam = new data_access_manager<output_type>(output.d);
			
			kalman = cvCreateKalman( DP, MP);
			// this doesn't work anyway
			//kalman->transition_matrix  = cvCreateMat( DP, DP, CV_32FC1 );
			//kalman->measurement_matrix = cvCreateMat( MP, DP, CV_32FC1 );
			
			memcpy( kalman->transition_matrix->data.fl, A, sizeof(A));
			memcpy( kalman->measurement_matrix->data.fl, H, sizeof(H));
			
			cvSetIdentity (kalman->process_noise_cov, cvRealScalar (1e-5));
			cvSetIdentity (kalman->measurement_noise_cov, cvRealScalar (1e-1));
			cvSetIdentity (kalman->error_cov_post, cvRealScalar (1));
			cvZero(kalman->state_post);
			cvZero(kalman->state_pre);
		}
		
		kalman_filter::~kalman_filter ()
		{
			cvReleaseKalman(&kalman);
			delete output.dam;
			return;
		}
		
		void kalman_filter::addInput(data_access_manager<input_type> * dk)
		{
			data_group<input_type> temp;
			temp.dam = dk;
			temp.d = temp.dam->checkOut();
			inputs.push_back( temp );
			
			dk->askForNotifications(this, kalman_filter_callback);
			
			return;
		}
		
		
		void kalman_filter::removeInput(data_access_manager<input_type> * dk)
		{
			vector<data_group<input_type> >::iterator i;
			
			for( i = inputs.begin(); i < inputs.end(); i++ )
			{
				if(i->dam == dk)
				{
					inputs.erase( i );
				}
			}
		}
		
		void kalman_filter::updateInputs()
		{
			
			vector<data_group<input_type> >::iterator i;
			
			for( i = inputs.begin(); i < inputs.end(); i++ )
			{
				i->d = i->dam->checkOut();
			}
			
			return;
		}
		
		data_access_manager<output_type>* kalman_filter::getOutputDAM()
		{
			return output.dam;
		}
		
		void kalman_filter::runFilter()
		{
			
			int meas=0;
			float Z[4];
			gps_base input;
			
			CvMat Zmat = cvMat (4, 1, CV_32F, Z);
			
			
			updateInputs();
			
			input = inputs[0].d;
			if(meas == 0)
			{
				kalman->state_post->data.fl[0] = input.Long();
				kalman->state_post->data.fl[1] = input.Lat();
				kalman->state_post->data.fl[2] = 0;
				kalman->state_post->data.fl[3] = 0;
				meas++;
			}
			else
			{
				Z[0] = input.Long();
				Z[1] = input.Lat();
				Z[2] = input.Long()-lastLong;
				Z[3] = input.Lat()-lastLat;
				cvKalmanCorrect(kalman, &Zmat);
				cvKalmanPredict(kalman, 0);
				cvMatMulAdd(kalman->measurement_matrix, kalman->state_pre, NULL, &Zmat);
				
				(output.dam)->checkIn(output.d);
				
			}
			
			lastLat  = input.Lat();
			lastLong = input.Long();
            
		}
		
		void kalman_filter::kalman_filter_callback(void * me, DataEventType type)
		{
			kalman_filter * newme = (kalman_filter*)me;
			newme->runFilter();
		}
		
   };
};