现设线性时变系统的离散状态防城和观测方程为:
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语言实现代码如下: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 , 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-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 */ 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;} #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(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; a[ii]=0.0; for (kk=0; kk<=n-1; kk++) a[ii]=a[ii]+p[i*n+kk]*f[j*n+kk]; } for (i=0; i<=n-1; i++) for (j=0; j<=n-1; j++) { ii=i*n+j; p[ii]=q[ii]; for (kk=0; kk<=n-1; kk++) p[ii]=p[ii]+f[i*n+kk]*a[kk*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; a[jj]=0.0; for (kk=0; kk<=n-1; kk++) a[jj]=a[jj]+p[i*n+kk]*h[j*n+kk]; } for (i=0; i<=m-1; i++) for (j=0; j<=m-1; j++) { jj=i*m+j; e[jj]=r[jj]; for (kk=0; kk<=n-1; kk++) e[jj]=e[jj]+h[i*n+kk]*a[kk*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; g[jj]=0.0; for (kk=0; kk<=m-1; kk++) g[jj]=g[jj]+a[i*l+kk]*e[j*m+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[i*n+j]*x[(ii-2)*n+j]; } for (i=0; i<=m-1; i++) { jj=i*l; b[jj]=y[(ii-1)*m+i]; for (j=0; j<=n-1; j++) b[jj]=b[jj]-h[i*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++) x[jj]=x[jj]+g[i*m+j]*b[j*l]; } if (ii<k) { for (i=0; i<=n-1; i++) for (j=0; j<=n-1; j++) { jj=i*l+j; a[jj]=0.0; for (kk=0; kk<=m-1; kk++) a[jj]=a[jj]-g[i*m+kk]*h[kk*n+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=i*l+j; b[jj]=0.0; for (kk=0; kk<=n-1; kk++) b[jj]=b[jj]+a[i*l+kk]*p[kk*n+j]; } for (i=0; i<=n-1; i++) for (j=0; j<=n-1; j++) { jj=i*l+j; a[jj]=0.0; for (kk=0; kk<=n-1; kk++) a[jj]=a[jj]+b[i*l+kk]*f[j*n+kk]; } for (i=0; i<=n-1; i++) for (j=0; j<=n-1; j++) { jj=i*n+j; p[jj]=q[jj]; for (kk=0; kk<=n-1; kk++) p[jj]=p[jj]+f[i*n+kk]*a[j*l+kk]; } } } free(e); free(a); free(b); return(js); }