卡尔曼滤波算法(C一一C++两种实现代码).docVIP

卡尔曼滤波算法实现代码 C++实现代码如下： ============================kalman.h================================//?kalman.h:?interface?for?the?kalman?class.////////////////////////////////////////////////////////////////////////#if?!defined(AFX_KALMAN_H__ED3D740F_01D2_4616_8B74_8BF57636F2C0__INCLUDED_)#define?AFX_KALMAN_H__ED3D740F_01D2_4616_8B74_8BF57636F2C0__INCLUDED_#if?_MSC_VER??1000#pragma?once#endif?//?_MSC_VER??1000#include?math.h#include?cv.h?class?kalman??{public:?void?init_kalman(int?x,int?xv,int?y,int?yv);?CvKalman*?cvkalman;?CvMat*?state;??CvMat*?process_noise;?CvMat*?measurement;?const?CvMat*?prediction;?CvPoint2D32f?get_predict(float?x,?float?y);?kalman(int?x=0,int?xv=0,int?y=0,int?yv=0);?//virtual?~kalman();};#endif?//?!defined(AFX_KALMAN_H__ED3D740F_01D2_4616_8B74_8BF57636F2C0__INCLUDED_)============================kalman.cpp================================#include?kalman.h#include?stdio.h/*?tester?de?printer?toutes?les?valeurs?des?vecteurs*//*?tester?de?changer?les?matrices?du?noises?*//*?replace?state?by?cvkalman-state_post?????*/CvRandState?rng;const?double?T?=?0.1;kalman::kalman(int?x,int?xv,int?y,int?yv){?????????cvkalman?=?cvCreateKalman(?4,?4,?0?);????state?=?cvCreateMat(?4,?1,?CV_32FC1?);????process_noise?=?cvCreateMat(?4,?1,?CV_32FC1?);????measurement?=?cvCreateMat(?4,?1,?CV_32FC1?);????int?code?=?-1;????????/*?create?matrix?data?*/?????const?float?A[]?=?{????1,?T,?0,?0,???0,?1,?0,?0,???0,?0,?1,?T,???0,?0,?0,?1??};??????????const?float?H[]?=?{?????1,?0,?0,?0,????0,?0,?0,?0,???0,?0,?1,?0,???0,?0,?0,?0??};? const?float?P[]?=?{????pow(320,2),?pow(320,2)/T,?0,?0,???pow(320,2)/T,?pow(320,2)/pow(T,2),?0,?0,???0,?0,?pow(240,2),?pow(240,2)/T,???0,?0,?pow(240,2)/T,?pow(240,2)/pow(T,2)????};?????const?float?Q[]?=?{???pow(T,3)/3,?pow(T,2)/2,?0,?0,???pow(T,2)/2,?T,?0,?0,???0,?0,?pow(T,3)/3,?pow(T,2)/2,???0,?0,?pow(T,2)/2,?T???};????????const?float?R[]?=?{???1,?0,?0,?0,???0,?0,?0,?0,???0,?0,?1,