Kalman滤波的多种实现方式浅析

一、卡尔曼滤波算法原理：

卡尔曼算法是个递推迭代的过程，若一个模型可以描述为线性过程，则可得到它的状态方程和观测方程。

建立状态方程：X(k)=F*X(k-1)+B*U(K-1)+W(k-1)

X(k)表示系统在k时刻的n维状态，U(K)是系统的控制量如若没有就设置为0，W(k)表示过程白噪声，符合正太分布，其方差为Q。

F是状态转移矩阵。

建立系统的观测方程：Z(K)=H*X(k)+V(k)

Z(K)表示系统在K时刻的系统的m维观测值，H是观测矩阵，V(k)表示观测白噪声，符合正太分布，其方差为R。

卡尔曼预测对象状态主要分为5个步骤：
1、根据上一个状态预测下一个状态：X(k+1|k)=F*X(k|k);

此时得到的状态仅仅是根据上一次的状态预测得到的，并没有利用此次的观测值对其进行校正，因此是不准确的。

2、协方差矩阵的预测：P(k+1|k)=F*P(k|k)*F'+Q; 

3、校正状态：X(k+1|k+1)=X(k+1|k)+Kg(k+1)*E(k+1);

利用观测值对其进行校正，Kg表示卡尔曼增益，E表示误差信息

4、计算卡尔曼增益：Kg(k+1)=P(k+1|k)*H'*inv [H*P(k+1|k)*H'+R];

5、协方差矩阵的更新：P(k+1|k+1)=[I-K(k+1)*H]*P(k+1|k);

若要详细了解其原理，请参考黄小平编著的《卡尔曼滤波原理及其应用-Matla仿真》P(36)_P(37);

二、卡尔曼滤波的实现三种实现方式：

本文引用书中的关于GPS定位的例子，建立系统的状态方程为[x(k),vx(k),y(k),vy(k)],是一个4维的状态空间模型，观测方程为2维[x(k),y(k)],因为GPS接收器能直接获取的只有目标的位置。假设目标做匀速直线运动，这个一个有二维观测向量来预测4维状态的模型。

实现方法1-matlab:

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%  Kalman滤波在船舶GPS导航定位系统中的应用
%  详细原理介绍及中文注释请参考：
%  《卡尔曼滤波原理及应用-MATLAB仿真》，电子工业出版社，黄小平著。
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function main
clc;clear;
T=1;
N=80/T;
X=zeros(4,N);
X(:,1)=[-100,2,200,20];
Z=zeros(2,N);
Z(:,1)=[X(1,1),X(3,1)];
delta_w=1e-2;
Q=delta_w*diag([0.5,1,0.5,1]) ;
R=100*eye(2);
F=[1,T,0,0;0,1,0,0;0,0,1,T;0,0,0,1];
H=[1,0,0,0;0,0,1,0];
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
for t=2:N
    X(:,t)=F*X(:,t-1)+sqrtm(Q)*randn(4,1);
    Z(:,t)=H*X(:,t)+sqrtm(R)*randn(2,1);  
end
Xkf=zeros(4,N);
Xkf(:,1)=X(:,1);
P0=eye(4);
for i=2:N
    Xn=F*Xkf(:,i-1);
    P1=F*P0*F'+Q;
    K=P1*H'*inv(H*P1*H'+R);
    Xkf(:,i)=Xn+K*(Z(:,i)-H*Xn);
    P0=(eye(4)-K*H)*P1;
end
for i=1:N
    Err_Observation(i)=RMS(X(:,i),Z(:,i));
    Err_KalmanFilter(i)=RMS(X(:,i),Xkf(:,i));
end
figure
hold on;box on;
plot(X(1,:),X(3,:),'-k');
plot(Z(1,:),Z(2,:),'-b.');
plot(Xkf(1,:),Xkf(3,:),'-r+');
legend('真实轨迹','观测轨迹','滤波轨迹')
figure
hold on; box on;
plot(Err_Observation,'-ko','MarkerFace','g')
plot(Err_KalmanFilter,'-ks','MarkerFace','r')
legend('滤波前误差','滤波后误差')
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function dist=RMS(X1,X2);
if length(X2)<=2
    dist=sqrt( (X1(1)-X2(1))^2 + (X1(3)-X2(2))^2 );
else
    dist=sqrt( (X1(1)-X2(1))^2 + (X1(3)-X2(3))^2 );
end

仿真结果如下图所示：

根据仿真结果可看出，卡尔曼预测值虽然不能完全替代真实值，但比测量值要误差要小的多。

实现方法2--C++&&OpenCV

利用OpenCV的Mat数据结果实现卡尔曼滤波的C++实现

#define  SAMPLES  60 //观测样本数

class Kalman_GPS
{
public:
Kalman_GPS();
~Kalman_GPS();
void KalmanPredict();
public:
Mat arrayX; //真实值矩阵
Mat arrayZ; //测量值矩阵
Mat arrayQ; //过程噪声方差，对角矩阵
Mat arrayR; //测量噪声方差，对角矩阵
Mat arrayXKF; //Kalman预测矩阵
Mat arrayF; //状态转移矩阵
Mat arrayH; //观测矩阵
};

/*
**constructor
*/
Kalman_GPS::Kalman_GPS()
{
arrayX=Mat::zeros(4,SAMPLES,CV_32FC1);//真实值
arrayZ=Mat::zeros(2,SAMPLES,CV_32FC1);//测量值
arrayXKF=Mat::zeros(4,SAMPLES,CV_32FC1);//kalman估计值


arrayQ=Mat::eye(4,4,CV_32FC1);//过程噪声方差
arrayR=Mat::eye(2,2,CV_32FC1);//测量噪声方差
arrayR=100*arrayR;

arrayQ.at<float>(0,0)=0.5;
arrayQ.at<float>(2,2)=0.5;
arrayQ=0.01*arrayQ;

/*初始状态*/
float tempdataX[4]={-100,2,200,20};
Mat TempX=Mat(4,1,CV_32FC1,tempdataX);
TempX.copyTo(arrayX.col(0));
/*初始测量值*/
float tempdataZ[2]={-100,200};
Mat TempZ=Mat(2,1,CV_32FC1,tempdataZ);
TempZ.copyTo(arrayZ.col(0));
}

/*
**destrustor
*/
Kalman_GPS::~Kalman_GPS()
{
;
}

/*
**kalman filter
*/
void Kalman_GPS::KalmanPredict()
{

float FData[4][4]={{1,1,0,0},
{0,1,0,0},
{0,0,1,1},
{0,0,0,1}};

float HData[2][4]={{1,0,0,0},
{0,0,1,0}};

arrayF=Mat(4,4,CV_32FC1,FData[0]); //状态转移矩阵
arrayH=Mat(2,4,CV_32FC1,HData[0]); //观察矩阵

RNG myrandn;
sqrt(arrayQ,arrayQ);//获得标准差
sqrt(arrayR,arrayR);
Mat GuassNoiseX(4,1,CV_32FC1);//过程高斯噪声矩阵
Mat GuassNoiseZ(2,1,CV_32FC1);//观测高斯噪声矩阵

for (int i=1;i<SAMPLES;i++)
{
myrandn.fill(GuassNoiseZ,RNG::NORMAL,0,1);
myrandn.fill(GuassNoiseX,RNG::NORMAL,0,1);
GuassNoiseX=arrayQ*GuassNoiseX;//得到相应方差的高斯矩阵
GuassNoiseZ=arrayR*GuassNoiseZ;

Mat TempX2=arrayF*arrayX.col(i-1)+GuassNoiseX;//根据状态方程F*X(k-1)+W(k)
TempX2.copyTo(arrayX.col(i));

Mat TempZ2=arrayH*arrayX.col(i)+GuassNoiseZ;//获得观测值H*x(k)+R(k)
TempZ2.copyTo(arrayZ.col(i));

}

/*kalman预测真实值*/
Mat arrayP0=Mat::eye(4,4,CV_32FC1);//协方差矩阵
arrayX.col(0).copyTo(arrayXKF.col(0));//初始化第一个状态
Mat TempEye=Mat::eye(4,4,CV_32FC1);

for(int i=1;i<SAMPLES;i++)
{
Mat TempKF1=arrayF*arrayXKF.col(i-1);//预测
Mat arrayP1=arrayF*arrayP0*(arrayF.t())+arrayQ; //预测协方差误差

Mat Temp2=arrayH*arrayP1*(arrayH.t())+arrayR;
Mat arrayKg=arrayP1*(arrayH.t())*(Temp2.inv());//卡尔曼增益

Mat TempKF2=TempKF1+arrayKg*(arrayZ.col(i)-arrayH*TempKF1);//预测值校正
TempKF2.copyTo(arrayXKF.col(i));

Mat tempP0=(TempEye-arrayKg*arrayH)*arrayP1;//协方差校正
tempP0.copyTo(arrayP0);
}

/*误差分析*/
Mat arrayErrObserv=Mat::zeros(1,SAMPLES,CV_32FC1);
Mat arrayErrKalman=Mat::zeros(1,SAMPLES,CV_32FC1);
Mat Dis_X_Xkl=arrayX-arrayXKF;

for(int i=0;i<SAMPLES;i++)
{
float Dis_ObsX=arrayX.at<float>(0,i)-arrayZ.at<float>(0,i);
float Dis_ObsY=arrayX.at<float>(2,i)-arrayZ.at<float>(1,i);
float Dis_KalX=Dis_X_Xkl.at<float>(0,i);
float Dis_KalY=Dis_X_Xkl.at<float>(2,i);

arrayErrObserv.at<float>(i)=sqrtf(Dis_ObsX*Dis_ObsX+Dis_ObsY*Dis_ObsY);
arrayErrKalman.at<float>(i)=sqrtf(Dis_KalX*Dis_KalX+Dis_KalY*Dis_KalY);
}

/*将数据导入到excel表格中*/
FILE* MyFile=NULL;
MyFile=fopen("test2.xls","w");

if(!MyFile)
{
cout<<"无法输出数据到表格"<<endl;
return;
}

for(int i=0;i<SAMPLES;i++)
{
fprintf(MyFile,"%2f\t",arrayX.at<float>(0,i));//真实的X方向位置坐标
fprintf(MyFile,"%2f\t",arrayX.at<float>(2,i));//真实的Y方向位置坐标
fprintf(MyFile,"%2f\t",arrayZ.at<float>(0,i));//测量的X方向位置坐标
fprintf(MyFile,"%2f\t",arrayZ.at<float>(1,i));//测量的Y方向位置坐标
fprintf(MyFile,"%2f\t",arrayXKF.at<float>(0,i));//卡尔曼预测X方向坐标
fprintf(MyFile,"%2f\t",arrayXKF.at<float>(2,i));//卡尔曼预测Y方向坐标
fprintf(MyFile,"%f\t",arrayErrObserv.at<float>(i));//测量误差
fprintf(MyFile,"%f\t",arrayErrKalman.at<float>(i));//预测误差
fprintf(MyFile,"\n");
}
fclose(MyFile);
}

因为C++无法像matlab那样有强大的显示函数，所以就先把数据导到excel表格中，进行绘图

实现方式3--直接利用OpenCV的卡尔曼类

未完待续

三、总结