卡尔曼滤波起源发展原理及应用

附录：kalman滤波（起源、发展、原理、应用）1、Kalman滤波起源及发展1960年，匈牙利数学家卡尔曼发表了一篇关于离散数据线性滤波递推算法的论文，这意味着卡尔曼滤波的诞生。斯坦利.施密特（StanleySchmidt）首次实现了卡尔曼滤波器，卡尔曼在NASA埃姆斯研究中心访问时，发现他的方法对于解决阿波罗计划的轨道预测很有用，后来阿波罗飞船的导航电脑使用了这种滤波器。关于这种滤波器的论文由Swerling（1958）、Kalman（1960）与KalmanandBucy（1961）发表.卡尔曼滤波是一种有着相当广泛应用的滤波方法，但它既需要假定系统是线性的，又需要认为系统中的各个噪声与状态变量均呈高斯分布，而这两条并不总是确切的假设限制了卡尔曼滤波器在现实生活中的应用。扩展卡尔曼滤波器EKF）极大地拓宽了卡尔曼滤波的适用范围。EKF的基本思路是，假定卡尔曼滤滤对当前系统状态估计值非常接近于其真实值，于是将非线性函数在当前状态估计值处进行台劳展开并实现线性化。另一种非线性卡尔曼滤波叫线性化卡尔曼滤波。它与EKF的主要区别是前者将非线函数在滤波器对当前系统状态的最优估计值处线性化，而后者因为预先知道非线性系统的实际运行状态大致按照所要求、希望的轨迹变化，所以这些非线性化函数在实际状态处的值可以表达为在希望的轨迹处的台劳展开式，从而完成线性化。不敏卡尔曼滤波器（UKF）是针对非线性系统的一种改进型卡尔曼滤波器。UKF处理非线性系统的基本思路在于不敏变换，而不敏变换从根本上讲是一种描述高斯随机变量在非线性化变换后的概率分布情况的方法。不敏卡尔曼滤波认为，与其将一个非线性化变换线性化、近似化，还不如将高斯随机变量经非线性变换后的概率分布情况用高斯分布来近似那样简单，因而不敏卡尔曼滤波算法没有非线性化这一步骤。在每一定位历元，不敏卡尔曼滤波器按照一套公式产生一系列样点，每一样点均配有一个相应的权重，而这些带权的样点被用来完整地描述系统状态向量估计值的分布情况，它们替代了原先卡尔曼滤波器中的状态向量估计值及协方差。不敏卡尔曼滤器让这些样点一一经历非线性状态方程与测量方程，然后再将这些经非线性变换后的样点按照它们的权重而综合出对当前时刻的系统状态向量估计值。多态自适应（MMA）卡尔曼滤波器是一种受到广泛关注的滤波器，它由好多个并联、同时运行的卡尔曼滤波器组成。在这组卡尔曼滤波器中，每一个滤波器对未知的滤波参数分别做出相互不同的假设，然后各自按照自己的模型假设进行滤波计算，而多态自适应滤波器最后将它们对系统状态的各个估计值进行加权，并以此作为最优估计值输出。

2、Kalman原理卡尔曼滤波是基于状态空间方法的一套递推滤波算法，在状态空间方法中，引入了状态变量的概念。实际应用中，可以通过选取合适的状态变量来体现系统的特征、特点和状况的变化。卡尔曼滤波的模型包括状态空间模型和观测模型。状态模型是反映状态变化规律的模型，通过状态方程来描写相邻时刻的状态转移变化规律；观测模型反映了实际观测量与状态变量之间的关系。Kalman滤波问题就是联合观测信息及状态转移规律来得到系统状态的最优估计。假设动态系统的状态空间模型为(2-1)(2-2)x(t+1)=ox(t)+rw(t)(2-1)(2-2)Y(t)=HX(t)+V(t)其中，X(t)系统在时刻t的状态Y(t)对状态的观测值W(t)系统噪声，方差阵为QV(t)观测噪声，方差阵为R① 状态转移矩阵H 观测矩阵r 系统噪声驱动矩阵卡尔曼滤波的计算流程为：计算状态估计值：(2-3)(2-4)(2-5)(2-6)X(t+111+1)=X(t+111)+K(t+(2-3)(2-4)(2-5)(2-6)计算状态一步预测：X(t+111)=QX(t11)计算新息：&(t+1)=Y(t+1)-HX(t+111)计算卡尔曼滤波增益：K(t+1)=P(t+111)Ht[HP(t+111)Ht+R]-1计算一步预测均方误差:(2-7)(2-8(2-7)(2-8)图1卡尔曼滤波的系统模型p(t+111)=①p(t11)Qt+rgrt计算一步预测估计均方误差:P(t+111+1)=[I—K(t+1)H]P(t+111)为了更形象地说明卡尔曼滤波的原理，下面给出卡尔曼滤波的系统模型框图:3、Kalman滤波应用之一：基于VDLL的GPS信号跟踪算法⑵导航滤波器导航滤波器图2VDLL的基本结构附录第3页系统的状态量为用户状态(位置、速度、用户钟差、用户钟漂移)，观测量为所有卫星通道的码环鉴相器输出组成的矢量D(X)X=(xyzbxyzb)tD(X)=(D(^t1)D(At2)…D(Atn))t (3-1)其中x,y,z，b,x,y,Z，b分别为在ECEF(EarthCenteredEarthFixed,地心坐标系)下的用户接收机三维位置和速度，b,b分别为接收机钟差和钟差变化率。预检测积分时间T选择为1ms,即滤波器迭代的k时刻与k+1时刻间隔1ms。假设用户为匀加速运动，可写出状态方程如下-1TI―Xk=0f (3-2)1-4 4」其中wk-1为状态扰动噪声。导航滤波器把k-1时刻的状态和对应的卫星位置信息反馈给本地信号生成模块，产生超前、即时与滞后码序列，分别与k时刻的1ms卫星信号的同相支路与正交支路做相关运算，即图2中的“积分存储”模块。为了去除对信号幅度变化的敏感性和对载波环路锁定的依赖性，码环鉴相器选择归一化的超前码与滞后码相关能量差(第i颗卫星)(3-3)D(金)='E2+QE2-IH-QLi，k IE2+QE2+IL2+QL(3-3)该码环鉴相器在没有噪声的理想情况下，当超前码与滞后码间距为1chip(码片)时，在±0.5chip输入误差范围内，鉴相器输出等于真实的跟踪误差。在实际应用中，超前滞后码的移位通常以采样点为单位，1个chip的长度不一定为整数个采样点，但鉴相器仍然与输入误差成线性关系，所以式可写为D(At)=gAt+v (3-4)i,k ii,k i,k其中gi为待求的鉴相器特性在线性区的斜率，v.k为观测噪声。鉴相器输入误差atk是k时刻(接收机时刻)第i颗卫星1ms长度的采样信号与本地即时码之间的相位偏移，而本地即时码的初始相位值等于k-1时刻对卫星信号的相位估计。所以△".,k实质上是k时刻卫星信号的相位与k-1时刻的相位估计之差，等于k时刻信号传输延迟的真实值与k-1时刻信号传输延迟的估计值。k时刻接收到第i颗卫星信号的理论测量延迟为TOC\o"1-5"\h\z1二 ; 7 ; 7 ;~ 7T=一\；(x -x)2+(y -y)2 +(Z -z)2 +b (3-5)i,k ck i,k k i,k ki,k k其中(xk,yk,zk)为k时刻用户的三维位置坐标,(X、k,yitk，zirk)为k时刻第i颗卫星的位置坐标,bk为k时刻的接收机钟差°K-1时刻信号传输延迟的估计值可由k-1时刻的用户状态估计X(k-1Ik-1)和卫星位置计算得到入 1 ： ■ — ■ — ■- ?t=—J(x —x)2+(y—x)2+(z—x)2+b(3-6)i,k—1 c k—1Ik—1 i,k—1 k—1Ik—1 i,k—1 k—1Ik—1 i,k—1 k—1Ik—1由(4))(6)式，可写出第i颗卫星的观测方程如下1二 - ； ；；I ~ 7广D(X)=g{-,(x-x)2+(y-y)2+(z-z)2+b-b-kic%k i,k k i,k ki,kk k—1Ik—1] (3-7)(x -x)2+(y-x)2+(z -x )2