卡尔曼滤波应用
最佳线性滤波理论起源于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程序：
1 #include "stdlib.h" 2 #include "rinv.c" 3 int kalman(n,m,k,f,q,r,h,y,x,p,g) 4 int n,m,k; 5 double f[],q[],r[],h[],y[],x[],p[],g[]; 6 { int i,j,kk,ii,l,jj,js; 7 double *e,*a,*b; 8 e=malloc(m*m*sizeof(double)); 9 l=m; 10 if (l<n) l=n; 11 a=malloc(l*l*sizeof(double)); 12 b=malloc(l*l*sizeof(double)); 13 for (i=0; i<=n-1; i++) 14 for (j=0; j<=n-1; j++) 15 { ii=i*l+j; a[ii]=0.0; 16 for (kk=0; kk<=n-1; kk++) 17 a[ii]=a[ii]+p[i*n+kk]*f[j*n+kk]; 18 } 19 for (i=0; i<=n-1; i++) 20 for (j=0; j<=n-1; j++) 21 { ii=i*n+j; p[ii]=q[ii]; 22 for (kk=0; kk<=n-1; kk++) 23 p[ii]=p[ii]+f[i*n+kk]*a[kk*l+j]; 24 } 25 for (ii=2; ii<=k; ii++) 26 { for (i=0; i<=n-1; i++) 27 for (j=0; j<=m-1; j++) 28 { jj=i*l+j; a[jj]=0.0; 29 for (kk=0; kk<=n-1; kk++) 30 a[jj]=a[jj]+p[i*n+kk]*h[j*n+kk]; 31 } 32 for (i=0; i<=m-1; i++) 33 for (j=0; j<=m-1; j++) 34 { jj=i*m+j; e[jj]=r[jj]; 35 for (kk=0; kk<=n-1; kk++) 36 e[jj]=e[jj]+h[i*n+kk]*a[kk*l+j]; 37 } 38 js=rinv(e,m); 39 if (js==0) 40 { free(e); free(a); free(b); return(js);} 41 for (i=0; i<=n-1; i++) 42 for (j=0; j<=m-1; j++) 43 { jj=i*m+j; g[jj]=0.0; 44 for (kk=0; kk<=m-1; kk++) 45 g[jj]=g[jj]+a[i*l+kk]*e[j*m+kk]; 46 } 47 for (i=0; i<=n-1; i++) 48 { jj=(ii-1)*n+i; x[jj]=0.0; 49 for (j=0; j<=n-1; j++) 50 x[jj]=x[jj]+f[i*n+j]*x[(ii-2)*n+j]; 51 } 52 for (i=0; i<=m-1; i++) 53 { jj=i*l; b[jj]=y[(ii-1)*m+i]; 54 for (j=0; j<=n-1; j++) 55 b[jj]=b[jj]-h[i*n+j]*x[(ii-1)*n+j]; 56 } 57 for (i=0; i<=n-1; i++) 58 { jj=(ii-1)*n+i; 59 for (j=0; j<=m-1; j++) 60 x[jj]=x[jj]+g[i*m+j]*b[j*l]; 61 } 62 if (ii<k) 63 { for (i=0; i<=n-1; i++) 64 for (j=0; j<=n-1; j++) 65 { jj=i*l+j; a[jj]=0.0; 66 for (kk=0; kk<=m-1; kk++) 67 a[jj]=a[jj]-g[i*m+kk]*h[kk*n+j]; 68 if (i==j) a[jj]=1.0+a[jj]; 69 } 70 for (i=0; i<=n-1; i++) 71 for (j=0; j<=n-1; j++) 72 { jj=i*l+j; b[jj]=0.0; 73 for (kk=0; kk<=n-1; kk++) 74 b[jj]=b[jj]+a[i*l+kk]*p[kk*n+j]; 75 } 76 for (i=0; i<=n-1; i++) 77 for (j=0; j<=n-1; j++) 78 { jj=i*l+j; a[jj]=0.0; 79 for (kk=0; kk<=n-1; kk++) 80 a[jj]=a[jj]+b[i*l+kk]*f[j*n+kk]; 81 } 82 for (i=0; i<=n-1; i++) 83 for (j=0; j<=n-1; j++) 84 { jj=i*n+j; p[jj]=q[jj]; 85 for (kk=0; kk<=n-1; kk++) 86 p[jj]=p[jj]+f[i*n+kk]*a[j*l+kk]; 87 } 88 } 89 } 90 free(e); free(a); free(b); 91 return(js); 92 }
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> 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; 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) ); 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){ state->data.fl[0]=x; state->data.fl[2]=y; cvRandSetRange( &rng, 0, sqrt(cvkalman->measurement_noise_cov->data.fl[0]), 0 ); cvRand( &rng, measurement ); cvMatMulAdd( cvkalman->transition_matrix, state, process_noise, cvkalman->state_post ); cvMatMulAdd( cvkalman->measurement_matrix, cvkalman->state_post, measurement, measurement ); 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; }
注：卡尔曼全名Rudolf Emil Kalman，匈牙利数学家，1930年出生于匈牙利首都布达佩斯。1953，1954年于麻省理工学院分别获得电机工程学士及硕士学位。1957年于哥伦比亚大学获得博士学位。我们现在要学习的卡尔曼滤波器，正是源于他的博士论文和1960年发表的论文《A New Approach to Linear Filtering and Prediction Problems》（线性滤波与预测问题的新方法）。如果对这编论文有兴趣，可以到这里的地址下载：http://www.cs.unc.edu/~welch/media/pdf/Kalman1960.pdf
还有个帖子，http://dzone.com/snippets/simple-kalman-filter-c，外国人写的简易卡曼滤波的C程序，值得参考。
