卡尔曼滤波原理及其应用

1、卡尔曼滤波卡尔曼滤波公式推导及应用摘要：卡尔曼滤波器是一个“optimal recursive data processing algorithm(最优化自回归数据处理算法)”。它能够从一系列的不完全及包含噪声的测量中，估计动态系统状态。对于解决大部分问题，它是最优、效率最高甚至是最有用的。它的的广泛应用已经超过30年，包括机器人导航、控制，传感器数据融合甚至在局势方面的雷法系统及导航追踪等等。近年来更被应用于计算机图像处理，例如头脸识别，图像分割，图像边缘检测等等。关键字：卡尔曼滤波 导航 机器人一 Kalmanl滤波器本质上来讲，滤波就是一个信号处理与变换（去除或减弱不想要的成分，增强所需

2、成分）的过程，这个过程既可以通过硬件来实现，也可以通过软件来实现。卡尔曼滤波属于一种软件滤波方法，基本思想是：以最小均方差为最佳估计准则，采用信号与噪声的状态空间模型，利用前一时刻的估计值和当前时刻的观测值来更新对状态变量的估计，求出当前时刻的估计值，算法根据建立的系统方程和观测方程对需要处理的信号做出满足最小均方差的估计。二 Kalman滤波起源及发展1960年，匈牙利数学家卡尔曼发表了一篇关于离散数据线性滤波递推算法的论文，这意味着卡尔曼滤波的诞生。斯坦利.施密特(Stanley Schmidt)首次实现了卡尔曼滤波器，卡尔曼在NASA埃姆斯研究中心访问时，发现他的方法对于解决阿波罗计划的

3、轨道预测很有用，后来阿波罗飞船的导航电脑使用了这种滤波器。关于这种滤波器的论文由Swerling (1958)、Kalman (1960)与 Kalman and Bucy (1961)发表.卡尔曼滤波是一种有着相当广泛应用的滤波方法，但它既需要假定系统是线性的，又需要认为系统中的各个噪声与状态变量均呈高斯分布，而这两条并不总是确切的假设限制了卡尔曼滤波器在现实生活中的应用。扩展卡尔曼滤波器（EKF）极大地拓宽了卡尔曼滤波的适用范围。EKF的基本思路是，假定卡尔曼滤滤对当前系统状态估计值非常接近于其真实值，于是将非线性函数在当前状态估计值处进行台劳展开并实现线性化。另一种非线性卡尔曼滤波叫线性

4、化卡尔曼滤波。它与EKF的主要区别是前者将非线函数在滤波器对当前系统状态的最优估计值处线性化，而后者因为预先知道非线性系统的实际运行状态大致按照所要求、希望的轨迹变化，所以这些非线性化函数在实际状态处的值可以表达为在希望的轨迹处的台劳展开式，从而完成线性化。不敏卡尔曼滤波器（UKF）是针对非线性系统的一种改进型卡尔曼滤波器。UKF处理非线性系统的基本思路在于不敏变换，而不敏变换从根本上讲是一种描述高斯随机变量在非线性化变换后的概率分布情况的方法。不敏卡尔曼滤波认为，与其将一个非线性化变换线性化、近似化，还不如将高斯随机变量经非线性变换后的概率分布情况用高斯分布来近似那样简单，因而不敏卡尔曼滤波

5、算法没有非线性化这一步骤。在每一定位历元，不敏卡尔曼滤波器按照一套公式产生一系列样点，每一样点均配有一个相应的权重，而这些带权的样点被用来完整地描述系统状态向量估计值的分布情况，它们替代了原先卡尔曼滤波器中的状态向量估计值及协方差。不敏卡尔曼滤器让这些样点一一经历非线性状态方程与测量方程，然后再将这些经非线性变换后的样点按照它们的权重而综合出对当前时刻的系统状态向量估计值。多态自适应（MMA）卡尔曼滤波器是一种受到广泛关注的滤波器，它由好多个并联、同时运行的卡尔曼滤波器组成。在这组卡尔曼滤波器中，每一个滤波器对未知的滤波参数分别做出相互不同的假设，然后各自按照自己的模型假设进行滤波计算，而多态

6、自适应滤波器最后将它们对系统状态的各个估计值进行加权，并以此作为最优估计值输出。三、Kalman原理卡尔曼滤波是基于状态空间方法的一套递推滤波算法，在状态空间方法中，引入了状态变量的概念。实际应用中，可以通过选取合适的状态变量来体现系统的特征、特点和状况的变化。卡尔曼滤波的模型包括状态空间模型和观测模型。状态模型是反映状态变化规律的模型，通过状态方程来描写相邻时刻的状态转移变化规律；观测模型反映了实际观测量与状态变量之间的关系。Kalman滤波问题就是联合观测信息及状态转移规律来得到系统状态的最优估计。假设动态系统的状态空间模型为 （2-1） （2-2）其中，X(t) 系统在时刻t的状态Y(t

7、) 对状态的观测值W(t) 系统噪声，方差阵为QV(t) 观测噪声，方差阵为R 状态转移矩阵H 观测矩阵 系统噪声驱动矩阵卡尔曼滤波的计算流程为：计算状态估计值： （2-3）计算状态一步预测： （2-4）计算新息： （2-5）计算卡尔曼滤波增益： （2-6）计算一步预测均方误差： （2-7）计算一步预测估计均方误差： （2-8）为了更形象地说明卡尔曼滤波的原理，下面给出卡尔曼滤波的系统模型框图：输入量测量噪声过程噪声观测量延时状态变量系统图1 卡尔曼滤波的系统模型四 Kalman滤波器的应用卡尔曼滤波器（Kalman Filter）是一个最优化自回归数据处理算法（optimal recursi

8、ve data processing algorithm）,它的广泛应用已经超过30年，包括航空器轨道修正、机器人系统控制、雷达系统与导航追踪等。近年来更被应用与组合导航与动态定位，传感器数据融合、微观经济学等应用研究领域。特别是在图像处理领域如头脸识别、图像分割、图像边缘检测等当前热门研究领域占有重要地位。1、Kalman滤波器在GPS系统中的应用GPS即全球定位系统(GlobalPositioningSystem)。简单地说，这是一个由覆盖全球的24颗卫星组成的卫星系统。这个系统可以保证在任意时刻，地球上任意一点都可以同时观测到4颗卫星，以保证卫星可以采集到该观测点的经纬度和高度，以便实现

9、导航、定位、授时等功能。这项技术可以用来引导飞机、船舶、车辆以及个人，安全、准确地沿着选定的路线，准时到达目的地。 GPS动态卡尔曼滤波算法系统主要应用在机动状态载体，因此采用最能描述载体强烈机动的当前统计模型。(1)状态方程建立。 所用GPS接收系统给出的是在地理坐标系中的经度、纬度和高度值。一般要先将地理坐标系转换为地球直角坐标系，并依次定义描述接收机在3个坐标轴的位置、速度、加速度、误差等信息. （3-1） （3-2） 分别为机动状态载体在3个坐标轴上的位置、速度、加速度和位置误差。通常采用一阶马尔科夫过程表示各个位置误差，总位置误差可看作有色噪声，则可表述为 （3-3） 上述3式中，

10、分别对应马尔科夫过程的相关时间常数。w为对应3条坐标上的高斯白噪声，则系统方程可写为(3)式中，系统的状态转移矩阵为 （3-4） （3-5） （3-6）(2)观测方程的建立将GPS接收机输出的定位结果为,那么在其中分别包括真实状态变量x，y，z和一阶马尔科夫过程误差和量测误差即 （3-7）若取 （3-8） （3-9）则矩阵形式为 （3-10）其中的观测矩阵可表示为 (3-11)由建立的方程得知，这是一个可观测的线性卡尔曼模型。(3)自适应卡尔曼滤波方程的建立。通过上述方程，可建立卡尔曼滤波方程如下 (3-12) (3-13) (3-14) (3-15) (3-16)其中， (3-17) (3-

11、18) (3-19) (3-20)式中 (3-21) (3-22) (3-23) (3-24)式中，Q(k)阵为系统的噪声协方差阵，Q的离散化矩阵。方程中(k+1)是引入的遗忘因子，可用来限制卡尔曼滤波器的记忆长度，改善滤波器的动态性能。卡尔曼滤波器是高斯过程最优滤波的一种有效算法，当对象模型足够准确，且系统状态和参数小，发生突变时，性能较好。但当模型存在误差，及系统状态和参数发生突变时，这种增长记忆滤波使“过老”的测量数据对现时的状态估计产生不良影响，甚至可能发散，从而影响滤波器的动态性能.。目标定位中的卡尔曼滤波算法一般的，目标跟踪系统方程可有以下式子表示其中U,V为零均值的过程噪声和观测

12、噪声，T采样周期滤波的五个核心公式（1）状态预测（2）协方差预测（3）卡尔曼增益（4）状态更新（5）协方差更新用卡尔曼滤波对移动目标进行定位，假设目标在二维平面上运动。初始位置为（-100m，200m），水平运动速度为20m/s，传感器的扫描周期为T=1s,观测噪声的均值为0，方差为3，过程噪声越小，目标越接近匀速直线运动，反之则为曲线运动.程序如下所示function mainclc;clear;T=1;N=60/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=d

13、elta_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);endXkf=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);

14、 Xkf(:,i)=Xn+K*(Z(:,i)-H*Xn); P0=(eye(4)-K*H)*P1;endfor i=1:N Err_Observation(i)=RMS(X(:,i),Z(:,i); Err_KalmanFilter(i)=RMS(X(:,i),Xkf(:,i); endfigurehold on;box on;plot(X(1,:),X(3,:),'-k'); plot(Z(1,:),Z(2,:),'-b.'); plot(Xkf(1,:),Xkf(3,:),'-r+'); figurehold on; box on;plot(

15、Err_Observation,'-ko','MarkerFace','g')plot(Err_KalmanFilter,'-ks','MarkerFace','r')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仿真结果如图所示：可以发现，通过卡尔曼滤波后误差大大减小.四 结论采用伪线性卡尔曼滤波算法，在参数估计的收敛速度和收敛精度上有明显的改善，在很大程度上克服了非线性问题线性化时，线性化误差导致的不良结果。通过伪量测变量的引入，对量测矩阵进行重新构造， 使得系统量测矩阵是量测角的函数，并且具有线性形式。该算法降低了对模型精度的要求，改进了扩展卡尔曼滤波的发散问题，具有较好的稳定性，在一定的误差范围内可以快速实现对空间目标定位。但伪线性卡尔曼滤波算法存在稳态有偏估计的问题，这有待于进