




下载本文档
版权说明:本文档由用户提供并上传,收益归属内容提供方,若内容存在侵权,请进行举报或认领
文档简介
1、实用标准文档卡尔曼滤波算法实现代码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&
2、gt;#include "cv.h"class kalmanpublic :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();
3、INCLUDED;#endif /!defined(AFX_KALMAN_H_ED3D740F_01D2_4616_8B74_8BF57636F2C0.kalman.cpp#include"kalman.h"#include<stdio.h>/*testerdeprintertoutes lesvaleurs desvecteurs*/*testerdechangerles matrices du noises*/* replace state by cvkalman->state_post ? */CvRandState rng;const double
4、 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 floatA口= 1,T,0,0,0,1,0,0,0,0,1,T,0,0,0,1;co
5、nst float H口 = 1,0,0,0,0,0,0,0,0,0,1,0,0,0,0,0;constfloatP口= 0,pow(320,2),pow(320,2)/T,0,0,pow(320,2)/T,pow(320,2)/pow(T,2),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
6、 float R口 = 1,0,0,0,0,0,0,0,0,0,1,0,0,0,0,0;cvRandInit( &rng, 0,1,-1, CV_RAND_UNI );cvZero( measurement );cvRandSetRange( &rng, 0,0.1,0 );rng.disttype = CV_RAND_NORMAL;cvRand( &rng, state );memcpy( cvkalman->transition_matrix->data.fl,A,sizeof (A);memcpy( cvkalman->measurement_m
7、atrix->data.fl,H, sizeof (H);memcpy( cvkalman->process_noise_cov->data.fl,Q, sizeof (Q);memcpy( cvkalman->error_cov_post->data.fl,P, sizeof (P);memcpy( cvkalman->measurement_noise_cov->data.fl,R, sizeof (R);/cvSetIdentity(cvkalman->process_noise_cov,cvRealScalar(1e-5);/cvSetI
8、dentity( cvkalman->error_cov_post, cvRealScalar(1);/cvSetIdentity( cvkalman->measurement_noise_cov, cvRealScalar(1e-1) );/* choose initial state */state->data.fl0=x;state->data.fl1=xv;state->data.fl2=y;state->data.fl3=yv;cvkalman->state_post->data.fl0=x;cvkalman->state_pos
9、t->data.fl1=xv;cvkalman->state_post->data.fl2=y;cvkalman->state_post->data.fl3=yv;0 );cvRandSetRange( &rng, 0, sqrt(cvkalman->process_noise_cov->data.fl0), cvRand( &rng, process_noise ); CvPoint2D32fkalman:get_predict( float x, float y) /* update state with current posit
10、ion */ state->data.fl0=x; state->data.fl2=y;/* predict point position */* x'k=A 敏 k+B 敏 kP'k=A 敏 k-1*AT + Q */cvRandSetRange( &rng, 0, sqrt(cvkalman->measurement_noise_cov->data.fl 0),0 );cvRand( &rng, measurement );/* xk=A?xk-1+B?uk+wk */cvkalmacvMatMulAdd( cvkalman->
11、transition_matrix,state, process_noise,measuren->state_post ); /* zk=H?xk+vk */ cvMatMulAdd( cvkalman->measurement_matrix, cvkalman->state_post, ment, measurement );cvKalmanCorrect( cvkalman, measurement );floatmeasured_value_x= measurement->data.fl0;floatmeasured_value_y= measurement-&g
12、t;data.fl2;const CvMat* prediction = cvKalmanPredict( cvkalman, 0 );floatpredict_value_x= prediction->data.fl0;floatpredict_value_y= prediction->data.fl2;return (cvPoint2D32f(predict_value_x,predict_value_y);void kalman:init_kalman( int x, int xv, int y, int yv)state->data.fl0=x;state->d
13、ata.fl1=xv;state->data.fl2=y;state->data.fl3=yv;cvkalman->state_post->data.fl0=x;cvkalman->state_post->data.fl1=xv;cvkalman->state_post->data.fl2=y;cvkalman->state_post->data.fl3=yv;c语言实现代码如下#include "stdlib.h"丽 田 TI 田I1 T 田 瞋In-I LI_1 r 融I 雨 T 崩#include "
14、;rinv.c"intlman(n,m,k,f,q,r,h,y,x,p,g)intn,m,k;doublef,q口川h,y口刈,p,g口; int i,j,kk,ii,l,jj,js;double *e,*a,*b;e=malloc(m*m* sizeof (double );l=m;if (l<n) l=n;a=malloc(l*l*sizeof (double );b=malloc(l*l*sizeof (double );for (i=0;i<=n-1;i+)for (j=0;j<=n-1;j+) ii=i*l+j; aii=0.0;for (kk=0;kk&
15、lt;=n-1;kk+)aii=aii+pi*n+kk*fj*n+kk;for (i=0;i<=n-1;i+)for (j=0;j<=n-1;j+) ii=i*n+j; pii=qii;for (kk=0;kk<=n-1;kk+)pii=pii+fi*n+kk*akk*l+j;for (ii=2;ii<=k;ii+) for(i=0;i<=n-1;i+)for (j=0;j<=m-1;j+) jj=i*l+j; ajj=0.0;题 T L 述II 卜I 帕 1 T 卜 I 酶 I 丁 上 丁 郎1for (kk=0;kk<=n-1;kk+)ajj=aj
16、j+pi*n+kk*hj*n+kk;)for(i=0;i<=m-1;i+)for(j=0;j<=m-1;j+) jj=i*m+j; ejj=rjj;for(kk=0;kk<=n-1;kk+)ejj=ejj+hi*n+kk*akk*l+j;)js=rinv(e,m);if (js=0) free(e); free(a); free(b); return (js);for(i=0;i<=n-1;i+)for(j=0;j<=m-1;j+) jj=i*m+j; gjj=0.0;for (kk=0;kk<=m-1;kk+)gjj=gjj+ai*l+kk*ej*m+kk
17、;for (i=0;i<=n-1;i+) jj=(ii-1)*n+i;xjj=0.0;for (j=0;j<=n-1;j+)xjj=xjj+fi*n+j*x(ii-2)*n+j;for (i=0;i<=m-1;i+) jj=i*l; bjj=y(ii-1)*m+i;for (j=0;j<=n-1;j+)bjj=bjj-hi*n+j*x(ii-1)*n+j;for (i=0;i<=n-1;i+) jj=(ii-1)*n+i;for (j=0;j<=m-1;j+)xjj=xjj+gi*m+j*bj*l;)if (ii<k) for (i=0;i<=n-1;i+)for (j=0;j<=n-1;j+) jj=i*l+j;a皿=0.0;for (kk=0;kk<=m-1;kk+)ajj=ajj-gi*m+kk*hkk*n+j;if (i=j)ajj=1.0+ajj;)for(i=0;i<=n-1;i+)for(j=0;j<=n-1;j+) jj=i*l+j;bjj=0.0;for(kk=0;kk<=n-1;kk+)bjj
温馨提示
- 1. 本站所有资源如无特殊说明,都需要本地电脑安装OFFICE2007和PDF阅读器。图纸软件为CAD,CAXA,PROE,UG,SolidWorks等.压缩文件请下载最新的WinRAR软件解压。
- 2. 本站的文档不包含任何第三方提供的附件图纸等,如果需要附件,请联系上传者。文件的所有权益归上传用户所有。
- 3. 本站RAR压缩包中若带图纸,网页内容里面会有图纸预览,若没有图纸预览就没有图纸。
- 4. 未经权益所有人同意不得将文件中的内容挪作商业或盈利用途。
- 5. 人人文库网仅提供信息存储空间,仅对用户上传内容的表现方式做保护处理,对用户上传分享的文档内容本身不做任何修改或编辑,并不能对任何下载内容负责。
- 6. 下载文件中如有侵权或不适当内容,请与我们联系,我们立即纠正。
- 7. 本站不保证下载资源的准确性、安全性和完整性, 同时也不承担用户因使用这些下载资源对自己和他人造成任何形式的伤害或损失。
最新文档
- 2025年物业管理服务合作协议书
- 眼科护士治疗操作规范与技能要点
- 安全培训讲师聘用合同及安全技能提升服务
- 海外紧急医疗救援与专业翻译协作协议
- 小红书品牌合作人资质审核及服务质量监管合同
- 美容护肤机构投资与品牌建设合同
- 跨区域品牌专柜委托经营管理合作协议
- 智能早教设备采购及教师数字化教学能力培养合同
- 旅游意外保险理赔处理协议
- 荷塘莲藕种植与农产品品牌推广委托管理协议
- 产业园 可行性研究报告
- 海外不动产买卖中介合同范本
- DB44-T 2605-2025 生活垃圾焚烧发电设施能源消耗计算与限额
- 2025江苏中考:化学必背知识点
- 漆房外协协议书
- 2025年能源行业能源需求预测与市场发展趋势2025
- 2024年“蓝桥杯”科学素养竞赛考试题库(含答案)
- 康复医疗复习题及参考答案
- 高标准农田项目规划设计方案
- 高血压科普基础知识培训-2025世界高血压日
- 2025春季学期国开电大专科《理工英语1》一平台在线形考(综合测试)试题及答案
评论
0/150
提交评论