[资料]

卡尔曼滤波简介＋算法实现代码

2016-9-21 12:10:54 2076

0 转最佳线性滤波理论起源于40年代美国科学家Wiener和前苏联科学家Ｋ*лмгрв等人的研究工作，后人统称为维纳滤波理论。从理论上说，维纳滤波的最大缺点是必须用到无限过去的数据，不适用于实时处理。为了克服这一缺点，60年代Kalman把状态空间模型引入滤波理论，并导出了一套递推估计算法，后人称之为卡尔曼滤波理论。卡尔曼滤波是以最小均方误差为估计的最佳准则，来寻求一套递推估计的算法，其基本思想是：采用信号与噪声的状态空间模型，利用前一时刻地估计值和现时刻的观测值来更新对状态变量的估计，求出现时刻的估计值。它适合于实时处理和计算机运算。现设线性时变系统的离散状态防城和观测方程为： X(k) = F(k,k-1)·X(k-1)+T(k,k-1)·U(k-1) Y(k) = H(k)·X(k)+N(k) 其中 X(k)和Y(k)分别是k时刻的状态矢量和观测矢量 F(k,k-1)为状态转移矩阵 U(k)为k时刻动态噪声 T(k,k-1)为系统控制矩阵 H(k)为k时刻观测矩阵 N(k)为k时刻观测噪声则卡尔曼滤波的算法流程为：预估计X(k)^= F(k,k-1)·X(k-1) 计算预估计协方差矩阵 C(k)^=F(k,k-1)×C(k)×F(k,k-1)'+T(k,k-1)×Q(k)×T(k,k-1)' Q(k) = U(k)×U(k)' 计算卡尔曼增益矩阵 K(k) = C(k)^×H(k)'×[H(k)×C(k)^×H(k)'+R(k)]^(-1) R(k) = N(k)×N(k)' 更新估计 X(k)~=X(k)^+K(k)×[Y(k)-H(k)×X(k)^] 计算更新后估计协防差矩阵 C(k)~ = [I-K(k)×H(k)]×C(k)^×[I-K(k)×H(k)]'+K(k)×R(k)×K(k)' X(k+1) = X(k)~ C(k+1) = C(k)~ 重复以上步骤其c语言实现代码如下： #include "stdlib.h" #include "rinv.c" int lman(n,m,k,f,q,r,h,y,x,p,g) int n,m,k; double f[],q[],r[],h[],y[],x[],p[],g[]; { int i,j,kk,ii,l,jj,js; double e,a,b; e=malloc(mmsizeof(double)); l=m; if (l a=malloc(llsizeof(double)); b=malloc(llsizeof(double)); for (i=0; i<=n-1; i++) for (j=0; j<=n-1; j++) { ii=il+j; a[ii]=0.0; for (kk=0; kk<=n-1; kk++) a[ii]=a[ii]+p[in+kk]f[jn+kk]; } for (i=0; i<=n-1; i++) for (j=0; j<=n-1; j++) { ii=in+j; p[ii]=q[ii]; for (kk=0; kk<=n-1; kk++) p[ii]=p[ii]+f[in+kk]a[kkl+j]; } for (ii=2; ii<=k; ii++) { for (i=0; i<=n-1; i++) for (j=0; j<=m-1; j++) { jj=il+j; a[jj]=0.0; for (kk=0; kk<=n-1; kk++) a[jj]=a[jj]+p[in+kk]h[jn+kk]; } for (i=0; i<=m-1; i++) for (j=0; j<=m-1; j++) { jj=im+j; e[jj]=r[jj]; for (kk=0; kk<=n-1; kk++) e[jj]=e[jj]+h[in+kk]a[kkl+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=im+j; g[jj]=0.0; for (kk=0; kk<=m-1; kk++) g[jj]=g[jj]+a[il+kk]e[jm+kk]; } for (i=0; i<=n-1; i++) { jj=(ii-1)n+i; x[jj]=0.0; for (j=0; j<=n-1; j++) x[jj]=x[jj]+f[in+j]x[(ii-2)n+j]; } for (i=0; i<=m-1; i++) { jj=il; b[jj]=y[(ii-1)m+i]; for (j=0; j<=n-1; j++) b[jj]=b[jj]-h[in+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++) x[jj]=x[jj]+g[im+j]b[jl]; } if (ii { for (i=0; i<=n-1; i++) for (j=0; j<=n-1; j++) { jj=il+j; a[jj]=0.0; for (kk=0; kk<=m-1; kk++) a[jj]=a[jj]-g[im+kk]h[kkn+j]; if (i==j) a[jj]=1.0+a[jj]; } for (i=0; i<=n-1; i++) for (j=0; j<=n-1; j++) { jj=il+j; b[jj]=0.0; for (kk=0; kk<=n-1; kk++) b[jj]=b[jj]+a[il+kk]p[kkn+j]; } for (i=0; i<=n-1; i++) for (j=0; j<=n-1; j++) { jj=il+j; a[jj]=0.0; for (kk=0; kk<=n-1; kk++) a[jj]=a[jj]+b[il+kk]f[jn+kk]; } for (i=0; i<=n-1; i++) for (j=0; j<=n-1; j++) { jj=in+j; p[jj]=q[jj]; for (kk=0; kk<=n-1; kk++) p[jj]=p[jj]+f[in+kk]a[jl+kk]; } } } free(e); free(a); free(b); return(js); } 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 #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 /* 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, 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_matrix->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) ); //cvSetIdentity( cvkalman->error_cov_post, cvRealScalar(1)); //cvSetIdentity( cvkalman->measurement_noise_cov, cvRealScalar(1e-1) ); / choose initial state / state->data.fl[0]=x; state->data.fl[1]=xv; state->data.fl[2]=y; state->data.fl[3]=yv; cvkalman->state_post->data.fl[0]=x; cvkalman->state_post->data.fl[1]=xv; cvkalman->state_post->data.fl[2]=y; cvkalman->state_post->data.fl[3]=yv; cvRandSetRange( &rng, 0, sqrt(cvkalman->process_noise_cov->data.fl[0]), 0 ); cvRand( &rng, process_noise ); } CvPoint2D32f kalman::get_predict(float x, float y){ / update state with current position / state->data.fl[0]=x; state->data.fl[2]=y; / predict point position / / x'k=A鈥k+B鈥k P'k=A鈥k-1AT + Q / cvRandSetRange( &rng, 0, sqrt(cvkalman->measurement_noise_cov->data.fl[0]), 0 ); cvRand( &rng, measurement ); /* xk=A?xk-1+B?uk+wk / cvMatMulAdd( cvkalman->transition_matrix, state, process_noise, cvkalman->state_post ); / zk=H?xk+vk / cvMatMulAdd( cvkalman->measurement_matrix, cvkalman->state_post, measurement, measurement ); / adjust Kalman filter state / / Kk=P'k鈥T鈥?H鈥'k鈥T+R)-1 xk=x'k+Kk鈥?zk-H鈥'k) Pk=(I-Kk鈥)鈥'k / cvKalmanCorrect( cvkalman, measurement ); float measured_value_x = measurement->data.fl[0]; float measured_value_y = measurement->data.fl[2]; const CvMat prediction = cvKalmanPredict( cvkalman, 0 ); float predict_value_x = prediction->data.fl[0]; float predict_value_y = prediction->data.fl[2]; return(cvPoint2D32f(predict_value_x,predict_value_y)); } void kalman::init_kalman(int x,int xv,int y,int yv) { state->data.fl[0]=x; state->data.fl[1]=xv; state->data.fl[2]=y; state->data.fl[3]=yv; cvkalman->state_post->data.fl[0]=x; cvkalman->state_post->data.fl[1]=xv; cvkalman->state_post->data.fl[2]=y; cvkalman->state_post->data.fl[3]=yv; } 4
举报淘帖0 只看该作者相关推荐 • 卡尔曼滤波算法对比其他的滤波算法有什么优点？ 474 • 卡尔曼滤波算法是怎么实现对数据的预测处理的？ 226 • 卡尔曼滤波简介 1854 • 卡尔曼滤波C代码 2633 • 卡尔曼滤波器的使用原理 1115 • 卡尔曼滤波的原理及如何实现 1128 • LabVIEW一维卡尔曼滤波算法 10851 • 图书分享：卡尔曼滤波算法的几何解释 12295 • 卡尔曼滤波 2809 • [共享] 卡尔曼滤波算法在TI TMS320C6713 DSP上的实现 9267 2条评论发表评论只看该作者