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

下载文档 降价啦

6
0
约7.33千字
约 10页
2017-09-30 发布于湖北
举报
版权申诉
保障服务

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

1、本文档共10页，可阅读全部内容。
2、原创力文档（book118）网站文档一经付费（服务费），不意味着购买了该文档的版权，仅供个人/单位学习、研究之用，不得用于商业用途，未经授权，严禁复制、发行、汇编、翻译或者网络传播等，侵权必究。
3、本站所有内容均由合作方或网友上传，本站不对文档的完整性、权威性及其观点立场正确性做任何保证或承诺！文档内容仅供研究参考，付费前请自行鉴别。如您付费，意味着您自己接受本站规则且自行承担风险，本站不退款、不进行额外附加服务；查看《如何避免下载的几个坑》。如果您已付费下载过本站文档，您可以点击这里二次下载。
4、如文档侵犯商业秘密、侵犯著作权、侵犯人身权等，请点击“版权申诉”（推荐），也可以打举报电话：400-050-0827(电话支持时间：9:00-18:30)。

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

卡尔曼滤波算法实现代码 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,