基于卡尔曼滤波的GPS与INS位置组合导航及matlab仿真

最优估计与滤波课程论文专业控制理论与控制工程学号132030044姓名方小星任课教师魏雪云2013年12月24日基于卡尔曼滤波的GPS/INS位置组合导航及matlab仿真方小星江苏科技大学电子与信息学院，镇江212003摘要：对GPS/INS位置组合导航系统用kalman滤波分析，并应用MATLAB软件仿真,从仿真的结果得出kalman滤波对组合导航中的轨迹滤波跟踪有真不错的效果。关键字：GPS/INS;位置组合导航;kalman;MATLAB0引言在任何系统中，为了对系统形成有效的控制，系统状态的准确把握显得尤为重要。我们可以通过一系列的手段对特定系统进行观测，以估计出系统的过去、现在、未来的状态，具体应用可分别表现为对过去状况的评估、当前状态的实时控制、趋势的准确预测等等。最优估计出系统状态过程中，实际的量测往往是存在诸如来自系统自身、测量工具等所带来的干扰，控制论中将这种干扰定义为噪声。如何去寻求滤除这种噪声干扰，便成为最佳系统状态估计首先必须解决的问题。Kalman滤波等一些滤波算法便因此应运而生，其作为一种最优估计理论与方法，由于它的实时递推、存储量小和简单易行的特点，在工程应用中受到了重视，广泛应用于信号处理、控制、通信、航天、制导、目标跟踪、石油勘探、故障诊断、卫星测控、GPS定位、检测与估计及机器人等等领域。卡尔曼滤波随时间及研究的发展，已形成了多种多样的理论和应用的形式。本次的学习带着了解认识该滤波算法思想和数学思维的目的，只对一般卡尔曼滤波问题进行了基本的研究。1Kalman滤波1.1离散系统的Kalman滤波问题设控制对象的离散模型为：(1)()()()()()()xkFxkGukvkykCxkwk（1-1）其中x(k)为n维状态向量，u(k)为m维控制向量，y(k)为r维输出向量，v(k)为n维过程干扰向量，w(k)为r维测量噪声向量。假设v(k)和w(k)均为离散的高斯白噪声序列，且有TT()0,()()()0,()()kjkjEvkEvkvjVEwkEwkwjW（1-2）其中：10kjkjkj（1-3）同时设V为非负定对称阵，W为正定对称阵，并设v(k)和w(k)不相关。这里我们现将v(k)看作白噪声序列，如果对于实际系统，v(k)不是白噪声，可通过功率谱密度等将其化为标准形式，这个在这里就不再赘述。在方程(1-1)中存在随机的干扰v(k)和随机的测量噪声w(k)，因此系统的状态向量x(k)也为随机向量，其中，y(k)是能够量测的输出量。问题是根据量测量y(k)估计出x(k)。若记x(k)的估计量为ˆ()xk，则ˆ()()()xkxkxk（1-4）为状态估计误差，因而T()()()PkExkxk（1-5）为状态估计误差的协方差阵，显然P(k)为非负定对称阵。这里估计的准则为根据量测量y(k),y(k-1),…最优地估计出ˆ()xk，以使P(k)极小〔P(k)是非负定对称阵，因而可以比较其大小〕。这样的估计称为最小方差估计。根据最优估计理论，最小方差估计为：ˆ()[()|(),(1),]xkExkykyk（1-6）即x(k)的最小方差估计ˆ()xk等于在给定的直到k时刻的所有量测量y的情况下x(k)的条件期望。为了后面推导的方便，下面引入更一般的记号：ˆ(|)[()|(),(1),]xjkExjykyk（1-7）根据j和k的大小关系，估计问题也可分为三类[1]：(a)jk称为预测（或外推）问题；(b)j=k称为滤波问题；(c)jk称为平滑（或内插）问题。为便于推导，我们引入如下记号：Tˆˆ()(|).........................................ˆ()()()...................................()()().................................ˆ(|1)....xkxkkkxkxkxkkPkExkxkkxkk时刻的状态估计时刻的状态估计误差时刻的状态估计误差协方差阵T............................................ˆ(|1)(|1)(|1).......(|1)(|1)(|1)......xkkxkkxkkPkkExkkxkk一步预报估计一步预报估计误差一步预报估计误差协方差阵1.2Kalman一步预报估计递推公式推导Kalman滤波器方程，我们可以先推导一步预报的方程，根据式(1-1)和(1-7)得：ˆ(1)[()(1),(2),]{[(1)(1)(1)](1),(2),}xkkExkykykEFxkGukvkykyk（1-8）根据定义很容易求得一步预报方程：ˆˆ(1)(1)(1)xkkFxkGuk（1-9）根据式(1-1)和(1-9)，可以得到一步预报误差：ˆ(1)()(1)ˆ[(1)(1)(1)][(1)(1)](1)(1)xkkxkxkkFxkGukvkFxkGukFxkvk（1-10）进一步求得一步预报估计误差的协方差阵：T(1)(1)(1)[(1)(1)][(1)(1)]PkkExkkxkkEFxkvkFxkvk（1-11）经展开整理可得：T(1)(1)PkkFPkFV（1-12）设x(k)的最小方差估计具有如下的形式：ˆˆˆ()(1)()[()(1)]ˆˆ(1)()[()(1)]xkxkkKkykykkxkkKkykCxxk（1-13）其中K(k)称为状态估计器或Ka1man滤波增益矩阵。该估计器方程具有明显的物理意义，式中第一项ˆ(1)xkk是x(k)的一步最优预报估计，它是根据直到k-1时刻的所有量测量的信息而得到x(k)的相关的最优估计。式中第二项是修正项，它根据最新的量测量信息y(k)来对最优预报估计进行修正。在第二项中ˆ(1)ykk是关于y(k)的一步预报估计，ˆˆˆ(1)()(1)()(1)ykkykykkykCxxk是关于y(k)的一步预报估计误差，也称新息（Innovation）。所以(13)式可以看作是一步最优预报和新息的加权平均。问题随之变成如何合适地选择K(k)。为获得x(k)的最小状态估计，令状态估计误差的协方差：TTˆˆ()()()[()()][()()]PkExkxkExkxkxkxk（1-14）最小。现在的问题变为：寻求K(k)使P(k)极小。可以证明，使P(k)极小等价于使如下的标量函数：T()()JExkxk（1-15）极小，其中J表示x(k)的各个分量的方差之和，因而它是标量。下面即按此准则来寻求K(k)。根据式(1-1)和(1-13)，可以求得x(k)的状态估计误差为：ˆ()()()ˆˆ()(1)()[()()(1)][()](1)()()xkxkxkxkxkkKkECxkwkCxkkIKkCxkkKkwk（1-16）进一步求得协方差阵：TTTT()()(){[()](1)()()}{[()](1)()()}[()](1)[()]()()PkExkxkEIKkCxkkKkwkIKkCxkkKkwkIKkCPkkIKkCKkWKk（1-17）可利用摄动法来寻找最优的K(k)，即给K(k)一个增量ΔK(k)，从而P(k)也变为P(k)+ΔP(k)，根据上式忽略高次项，可以求得：TTTTTT()()()[()](1)(())(())(1)[()]()()()()()()KKKPkPkPkIKkCPkkKkCKkCPkkIKkCKkWKkKkWKkKkRRKk（1-18）其中：T[()](1)()RIKkCPkkCKkW（1-19）如果K(k)能够使得式(1-17)中的P(k)取极小值，那么，对于任意的增量ΔK(k)均应有ΔP(k)=0。要使该点成立，则必须有TTT[()](1)()(1)()[(1)]0RIKkCPkkCKkWPkkCKkCPkkCW（1-20）即：TT1()(1)[(1)]KkPkkCCPkkCW（1-21）最后，归纳上边(1-9)(1-13)(1-21)(1-12)(1-17)即构成了Ka1man滤波递推公式。关键是Kalman增益矩阵的求解，求得增益后再利用式(1-9)和(1-13)便可以轻松的到状态的最优估计了。1.3增益矩阵计算的迭代法增益矩阵调可以直接根据式Kalman递推公式进行计算，下面给出迭代计算的程序流程：①给定参数F,C,V,W和P(0)，给定迭代计算总步数N，置k=1；②按式(1-12)计算P(k|k-1)；③按式(1-21)计算K(k)；④按式(1-17)计算P(k)；⑤如果k=N，转⑦；否则转⑥；⑥k=k+1，转②；⑦输出K(k)和P(k)，k=1,2,…,N。在以上的计算过程中，将式(1-17)展开得TTTTTTTTT()(1)()(1)(1)()()(1)()()()[()](1)(1)()()[(1)]()PkPkkKkCPkkPkkCKkKkCPkkCKkKkWKkIKkCPkkPkkCKkKkCPkkCWKk（1-22）式中：第三项可由(1-20)式解出TTTT()[(1)]()(1)()KkCPkkCWKkPkkCKk，于是可得：()[()](1)PkIKkCPkk（1-23）可见，式(1-23)要比式(1-17)简单得多，因此可在上面的程序流程中用式(1-23)代替式(1-17)，这样可以减少计算工作量。但有一点必须注意，P(k)是对称阵，式(1-17)具有明显的对称的形式，因此在迭代计算过程中，即使由于存在救值计算的误差也不致破坏P(k)的对称性，这是利用式(1-17)进行计算的优点；而利用式(1-23)进行计算时，虽然计算比较简单，但是由于数值计算误差的存在，有可能破坏P(k)的对称性，这是具体计算时应该注意的问题。2组合导航系统状态方程和观测方程的建立卡尔曼滤波在工程实践中，特别在航空空间技术中迅速得到应用。例如在测轨问题和惯性导航等方面都应用卡尔曼滤波。所谓测轨问题，就是在不同时刻，对飞行器进行观测，根据观测数据，应用卡尔曼滤波方法，估计出这个飞行器每时每刻的状态变量，如飞行器的位置、速度、加速度及阻力系数等物理量。以便对飞行器进行导航、制导和拦截。本文采用GPS/INS组合导航的方法进行轨迹滤波。当组合系统采用线型卡尔曼滤波器时，常常利用间接法进行估计，所谓“系统”实际上是导航系统的各种误差的“组合体”，滤波器估值的主要部分是导航参数误差估值.X，然后用.X去校正惯导系统INS的导航参数。惯性导航系统的误差方程包括：平台误差角方程；速度误差方程；位置误差方程等。2.1平台误差角方程惯性导航系统的平台系要模拟的导航坐标系为当地地理坐标系(t),由于平台有误差，平台系(P)和导航系(t)不能完全重合，平台系(P)与地理系(t)总存在小姿态误差角将平台误差角写成(t)系中的列向量有：TtENU地理系(t)和平台系(P)之间的坐标变换阵，取一阶近似，可写成：111UNptUENEC（2-1）在捷联惯导系统中，由于误差的影响理想的当地地理系与计算所得的地理系有小角度误差，坐标变换阵与上式完全一样，即：'tpttCC平台式INS中，平台相对于惯性空间的转动角速度为：..1pptttttiptititC（2-2）.X.X亦可写成:.1tpttipit又pppipic其中：picp施矩角作用下的跟踪转动陀螺漂移引起的平台等效误差转动上式有关项可写成：