专利名称：一种用于惯性组合导航系统中的改进混合高斯粒子滤波方法
技术领域：
本发明是一种用于惯性组合导航系统中新的改进混合高斯粒子滤波方法，属于多 传感器信息融合技术，是一种用于多传感器数据融合方法。该专利内容同样适用于惯性组 合导航、目标识别与跟踪、图像处理、模式识别等其他多传感器信息融合及多源数据处理应 用领域。
背景技术：
惯性导航系统自主导航能力强，同时系统动态性能好，数据输出更新频率高，在短 期内导航精高。惯导的这些优点，使得它在军事、民用等等领域有着广泛的应用。但是由于 惯导的导航参数误差存在随时间而积累的缺点，长时间独立工作效果差，一般通过组合导 航的方式来解决误差积累问题。比如常见的有捷联惯导和全球卫星定位系统构成的SINS/ GPS组合导航系统，通过GPS辅助从而提高组合系统的导航性能。由于GPS输出的位置、速度 信息与时间相关，松组合存在一定的模型误差，甚至会出现滤波发散现象。相比较而言，伪 距、伪距率的深组合方式在滤波精度和系统可靠性方面有一定的优势。在深组合模式下，有 的情况观测方程中会出现非线性环节，不便采用传统的线性卡尔曼滤波技术直接处理。理 论上，粒子滤波能适用于非高斯非线性系统，可以解决上述SINS/GPS深组合导航系统中出 现的非线性滤波问题。但由于组合导航系统状态量维数高，导航系统有实时性要求，目前大 部分粒子滤波都难以在组合导航系统中直接应用。需要根据组合导航模型特点对粒子滤波 算法进行适当的改进和优化，改进的目的是为了减少粒子滤波的计算量、提高滤波精度等， 使其能更好的满足组合导航系统的要求。在SINS/GPS深组合导航系统中，当采用导航参数误差量组成状态方程，用GPS伪 距构成观测方程时，滤波系统中会出现混合模型。在混合模型中，状态方程可能是线性或弱 非线性的，观测方程为一般非线性或强非线性。针对上述混合模型采用单一的滤波方法效 果会受到影响，如果采用线性卡尔曼滤波或非线性的EKF或UKF，滤波精度会受到观测方程 中强非线性模型的影响。如果采用一般的粒子滤波方法，由于滤波维数高，选取的粒子数目 会比较大，计算量大，难以满足组合导航系统的实时型要求。如何在组合导航的混合模型系 统中，采用混合滤波方法，把模型和多种数据处理方式有机的结合起来是组合导航系统滤 波方法的难点之一，本发明专利是针对上述情况的一种有效组合导航系统数据处理方法。

发明内容
技术问题在组合导航系统中，当状态方程出现弱非线性，观测方程非线性的情况 下，本发明专利提出了一种新的混合滤波方法。该方法的主要思路是在GPF滤波框架下，有 机的把UKF和粒子滤波有机的结合起来，在减少粒子滤波的计算量的同时，提高了系统的 滤波精度。本发明为实现上述目的，采用如下技术方案
本发明一种用于惯性组合导航系统中的改进混合高斯粒子滤波方法，包括下列步 骤(1)建立组合导航系统的状态方程、观测方程及噪声模型；(2)建立符合步骤(1)所述噪声模型的高斯粒子滤波算法；(3)组合导航系统的状态方程为弱非线性的，采用UKF滤波方法获取混合高斯粒 子滤波状态更新过程中的高斯分布参数，更新组合导航系统的状态方程；(4)采用步骤(2)所述高斯粒子滤波算法实现组合导航系统的状态更新及其余部 分解算；(5)由导航计算机根据步骤(4)更新后的导航系统的状态方程和观测方程，完成 组合导航的数据处理及解算。优选地，步骤(3)所述的高斯分布参数获取方法如下(a)初始化将过程噪声和量测噪声增广为状态向量，增广后的状态向量为xa，相 应的采样点向量为>^，&为原状态向量协方差初始估计值，Pv是过程噪声方差，？ 是量测噪 声方差，在该状态扩展过程中，把陀螺及加速度的有色噪声按状态扩展法转化为白噪声；(b)计算采样点根据状态量其对应的方差厂构造一个n行，2n+l列的矩 阵，用定点采样的方式获取代表状态量的2n+l各Sigma点；(c)时间更过程根据步骤(b)获取的Sigma点和系统方程，利用KF状态更新公 式获取系统的状态预测量毛和量测预测量之/w ；(d)量测更新过程根据步骤(c)的到的时间根系预测信息和最新的观测量zk，利用KF量测更新公式获取状态量的最终滤波状态量毛及其对应的方差Pk ；这样通过UKF获取了更新后状态量的多维高斯分布参数，为粒子滤波提供状态量 及噪声的分布参数；其中，xa表示Sigma点集，Pa为其对应方差；毛为状态预测值，l/w为量测预测 值；毛为状态量滤波值，Pk为其对应的方程。优选地，步骤(4)所述的实现组合导航系统的状态更新及其余部分解算的方法如 下Stepl 初始化选取步骤(3)中合适的高斯分布参数，使高斯粒子服从N(X(I ； u 0, E 0)正态分布；Step2 时间更新过程求出步骤(3)所述UKF滤波方法更新组合导航系统的状态 后的正态分布参数，使更新后的高斯粒子服从
正态分布；Step3:量测更新过程根据步骤St印2所述更新后的高斯粒子的分布情况
抽样得到更新后的高斯粒子仄
;计算高斯粒子权值
一化高斯粒子权值巧⑴； Step4 计算滤波及相关参数根据步骤St印3所述高斯粒子权值跟新状态分布， 使量测更新后的粒子服从N(xnz ； u n, E n)正态分布，并得到状态量的滤波值y n及其相应
的方差 ∑ n ；其中 St印5 返回步骤(2)；
其中，N(x;U,E )表示标准多维高斯分布，x, U，E分别表示对于的状态量、均 值和均方差；N(X(1 ； u 0, E 0)表示状态量的初始分布，无;从，ij表示状态更新后的分布； 次巧；！表示状态量的样本点集，表示其对应的权值《nw。有益效果本发明的方法具有如下优点根据组合导航滤波模型的特点，当状态方程为弱非 线性，观测方程为非线性时，把高斯粒子滤波分为两个部分分步实现。首先采用UKF获取状 态根系后的状态量多维高斯分布的分布参数，在得到最新观测量后，利用粒子滤波算法原 理获取粒子更新后的权值，并计算最终滤波结果。该方法一方面提高了组合导航系统的滤 波精度，同时滤波时间也有一定的减少，能够较好的解决组合导航系统的非线性滤波问题。 对以上发明的有益效果说明如下在同等条件下，导航计算机根据传感器数据和系统模型，采用不同的滤波算法进 行导航数据融合，粒子数都选用2700个。将本发明提出的新的混合高斯粒子滤波算法与 普通高斯粒子滤波算法进行组合导航系统数据处理并对结果进行对比。图2、3、4为高度、 经度、纬度方向上的位置误差的数据比较曲线。其中①为普通高斯粒子滤波器用于组合导 航系统的滤波结果，②为改进混合高斯粒子滤波在组合导航系统中的滤波结果。由图2、3、 4可以看出，改进混合高斯粒子滤波算法在滤波精度上有一定的优势，其位置误差为11.5 米，与普通高斯粒子滤波的15. 2米相比，滤波精度有了较大的提高。图5给出了两种滤波 方式在不同粒子数目下的平均单步滤波计算时间，其中①为普通高斯粒子滤波器的滤波时 间，②为改进混合高斯粒子滤波的滤波时间。由图可以看出改进混合高斯粒子滤波的滤波 时间有所减少，其滤波时间约为普通高斯粒子滤波时间的75%。根据系统的滤波模型特点， 采用混合滤波粒子滤波方法，在组合导航系统的数据处理中，滤波精度有了提高，同时滤波 时间也有所减少，该方法能更好的满足组合导航系统数据处理的需要，具有一定的理论和 实际应用价值。


图1是改进混合高斯粒子滤波算法流程框图。图2是高度误差比较曲线。图3是纬度误差比较曲线。图4是经度误差比较曲线。图5是滤波时间比较图。
具体实施例方式下面结合附图对发明的技术方案进行详细说明如图1所示，首先根据惯性组合导航的特点建立SINS/GPS组合导航系统的状态方 程、观测方程及噪声模型；根据该模型特点，建立符合要求的混合高斯高斯粒子滤波算法结 构。根据上述算法结构，把粒子滤波分为两步，采用UKF算法获取混合高斯粒子滤波状态更 新过程中的高斯分布参数。采用粒子滤波算法实现状态更新及其余部分算法，并给出混合 高斯粒子滤波的实现算法流程。由导航计算机根据惯性组合导航的滤波模型及算法流程， 完成组合导航的数据处理及解算工作。
1)建立组合导航状态方程、观测方程及噪声模型选取滤波状态量为X = [ (K 6V, 6L, 6 A , 6 h, b, d]，根据传感器误差模型及 惯导误差方程建立状态方程。其中小=[小6，小 ，小』为平台误差角，Sv = [SVe，SVn， Svu]表示速度误差，SL，6 A, Sh表示位置误差。关于上述状态量的微分方程及其离 散化可以参考文献。b，d表示深组合时GPS的时钟误差，其中b表示等效时钟误差，d表 示等效时钟频率误差，其对应的微分方程是+ % ,d = -d/Td+wd。其中状态方程的 噪声为W = [wgx, wgy, wgz, wax, way, waz, wb, wd]0根据上述描述，可以列出系统的状态方程 x(o=mm+G(t)w。在该系统中，陀螺及加速度计的有色噪声没有通过状态扩展法处理， 是有色噪声，该状态方程具有弱非线性。该有色噪声将在粒子滤波的参数求解和权值更新 过程中单独处理，目的是降低系统状态量的维数。设载体在地球大地坐标系统的位置经度、纬度和高度是\、L、h，其在地球直角坐 标系的对应的位置x、y、z可由(1)式表示。第i颗卫星对应的伪距可由(2)式表示。
设系统的观测量
代入(1)式，再把(1)式代入(2)式，就可以得到系统的观测方程。根据上述说明，可以列 出系统的观测方程Z(t) =h(X(t))+V(t)。2)建立符合模型特点的混合高斯粒子滤波算法结构高斯粒子滤波是一种非线性粒子滤波，能较好的解决非线性滤波问题，为了便于 实现混合粒子滤波，需把高斯粒子滤波分解为较为独立的两个部分分步实现。利用UKF获 取状态更新后的高斯分布参数，采用高斯粒子滤波实现粒子权值更新，从而获取最终的滤 波值。设离散系统后验密度函数的求解可由递推贝叶斯估计公式给出，该过程可总结为如 下公式。
_!(预测“时间更新)(3)
)(滤波 _ 量测更新)(4)其中 为归一化因子。假设在时刻 n，p(xn|y0: -)可由(5)式的高斯分布表示
p(xn \yv,-x) = \p(xn ) p (Vi k —!) Nixn ；" ,瓦)⑶当得到的n时刻的量测值yn后，⑷式的滤波密度函数可由(6)式表示，p(x \yin) = C p(x |凡—)p(yn \xn) C iV(x \Jin,\)p[yn \xn)(6)p(x \yln)^N(xn-,jun,i:n)(7)在高斯粒子滤波的实现过程中，上述算法可以总结为两个阶段，首先根据状态方 程和相关参数，获取粒子的高斯分布参数，其次首先如果求出该密度函数，滤波问题也得到 解决。假设(6)式的滤波密度函数可由(7)式高斯分布来逼近。只要求取(5)和(7)式高 斯分布的参数，就可以计算出滤波结果。
7
对于一般的粒子滤波，无论是状态方程还是量测方程都是非线性的表达形式，描 述了系统的一般形式，适合于通用的非线性应用场合。在上述组合导航滤波模型中，系统方 程是弱非线性的，只是系统的噪声是非高斯的，观测方程是强非线性的。在这种模型中，采 用常用的粒子滤波算法，计算量比较大，难以满足组合导航系统实时性要求。同时由于粒子 数目难以满足粒子滤波的基本要求，滤波精度也会受到影响。在这种特殊的系统模型中，本 专利针对基本高斯粒子滤波提出了一种改进新算法。改进的主要方法是，利用UKF算法实 现高斯粒子滤波的状态更新过程，获取高斯分布的参数，保留高斯粒子滤波中的量测更新 及其它计算过程。在传统的高斯粒子滤波状态更新过程中，需要通过状态方程及其噪声分 布对每个粒子分别进行状态更新，而改进算法在状态更新的过程中，只需采用UKF对状态 参数进行更新。在新算法中，由于UKF只需较少的sigma点就能获取精度较高的状态更新 参数，所以算法的滤波精度也有一定的提高，同时计算量有明显的减少。3)采用UKF算法获取GPF状态更新过程中的高斯分布参数从改进混合高斯粒子滤波算法的结构上看，改进混合高斯粒子滤波的实现分为两 步，时间更新和量测更新。在时间更新部分，根据系统的状态方程和噪声分布情况，获取根 系后粒子的分布参数。在量测更新部分，根据粒子滤波算法修改粒子的权值，从而得到更加 接近后验概率分布的分布参数，获取最终的滤波结果。针对组合导航系统中状态方程弱非 线性的情况，本专利提出了采用UKF获取量测更新过程中的高斯分布参数。其具体实现方 法如下UKF滤波器分为以下几个步骤a)初始化将过程噪声和量测噪声增广为状态向量，增广后的状态向量为x%相应的采样点 向量为x % P0为原状态向量协方差初始估计值，Pv是过程噪声方差，Pn是量测噪声方差。
(8) 在该状态扩展过程中，把陀螺及加速度的有色噪声按状态扩展法转化为白噪声处 理，由于观测噪声在系统方程的建立过程中已经扩展在状态量里，这里无须额外处理。b)计算采样点构造一个n行(n为增广状态向量的维数)，2n+l列的矩阵，各列形式如下
(13)
(14)
(15)这里，入=a2(n+k)-n, a决定采样点距均值的远近程度，通常被赋一个较小的 正值，k彡0保证方差阵的半正定性。
(10) (11)
(12)
c)时间更新方程 上式中f为系统的非线性状态方程式。
h为系统的非线性量测方程式。
其中


0用于包含状态量分布的高阶成分信息，0彡0。d)量测更新方程
通过UKF获取了更新后状态量的多维高斯分布参数，其中xT的均值及对应的方差 代表了导航参数X= [(K 6V, 6L, 6 A, 6h,b,d]的状态分布参数。同时可以获取噪声 的相关参数及分布，为粒子滤波提供状态量及噪声的分布参数。4)采用粒子滤波算法原理和改进措施设计出符合要求的混合高斯粒子滤波算法 流程通过上述UKF滤波方法获取了导航参数状态更新后的状态量及噪声分布参数，利 用这些参数，进行粒子抽样和权值更新并最终获取滤波值。算法的主要思想是利用UKF算 法实现高斯粒子滤波的状态更新过程，获取高斯分布的参数，保留高斯粒子滤波中的量测 更新及其它计算过程。其中状态量的多维高斯分布参数已经通过UKF的方法获取，粒子的 抽样可以根据状态量更新后的分布参数#(毛;来获取，只是需要注意对有色噪声按 噪声模型进行还原处理。在粒子抽样完成后，更加粒子滤波权值更新公式求得更新后的粒 子权值，根据更新的粒子及权值从而得到状态量的滤波值。由于改进算法在状态更新的过 程中，只需要对其高斯分布参数及均值和方差进行更新，这样计算量就大大降低了。在新算 法中，由于UKF只需较少的sigma点就能获取精度较高的状态更新参数，所以算法的滤波精度也有一定的提高，同时，计算量有明显的减少。通过上述说明可以给出该算法的实现流 程。如下Stepl 初始化选取合适的参数，使粒子服从N(xq ； U0,E 0)正态分布；St印2 时间更新过程根据上次滤波结果系统的状态方程，利用UKF滤波方法获 取更新后状态量的分布参数，状态更新公式分别求出状态更新后的正态分布参数，使更新 后的粒子服从7V(毛；从,i: )正态分布。St印3 量测更新过程根据更新后粒子的分布情况久，良)抽样得到 更新后的粒子，表示为丨yii ；根据公式
计算粒子权值；根据公式 St印4 计算滤波及相关参数根据粒子及相应权值计算滤波结果和相关参数，使 量测更新后的粒子服从则1;1^，[11)正态分布，并得到状态量的滤波值及其相应的方 St印5 从第(2)步重复上述步骤，循环以上步骤。从上叙流程可以看出，首先对改进混合高斯粒子滤波进行初始化，使得状态量的 初始值为导航系统的初始对准值。其次要进行粒子抽样，抽样的方法是以UKF滤波后的均 值为粒子的均值，根据噪声模型产生粒子附加噪声，然后根据状态方程模型获取粒子。5)完成组合导航的数据处理及解算工作由导航计算机根据组合导航系统的状态方程和观测方程，IMU的输出及GPS的数 据完成组合导航的数据融合工作，并完成组合导航数据处理解算。导航系统的初值由初始对准的结果给出，航迹及各传感器的数据输出有相应的模 拟器给出，根据导航系统的解算原理及改进混合高斯滤波方法解算出导航参数的滤波结 果。其中改进混合高斯粒子滤波算法把高斯粒子滤波分为两步，当组合导航系统的状态方 程为弱非线性是，采用UKF滤波算法获取系统状态量的多维高斯分布参数。得到新的观测 量后粒子高斯粒子滤波技术获取更新后的权值，从而得到导航参数的滤波值。改进混合高 斯粒子滤波在上述模型下，提高了滤波精度，同时滤波时间也有一定的减少。在同等条件下 将改进混合高斯粒子滤波算法与普通高斯粒子滤波算法进行组合导航系统数据处理的对 比。图2、3、4所示为普通高斯粒子滤波算法和基于UKF的改进高斯粒子滤波算法在组 合导航数据处理中的应用结果比较，其中①为普通高斯粒子滤波器用于组合导航系统的滤 波结果，②为改进混合高斯粒子滤波在组合导航系统中的滤波结果。由图2、3、4可以看出， 改进混合普通粒子滤波算法在滤波精度上有一定的优势，其位置误差为11. 5米，与普通高 斯粒子滤波的15. 2米相比，滤波精度有了一定的提高。图5给出了两种滤波方式在不同粒 子数目下的平均单步滤波计算时间，其中①为普通高斯粒子滤波器的滤波时间，②为改进 混合高斯粒子滤波的滤波时间。由图可以看出改进混合高斯粒子滤波的滤波时间有所减 少，其滤波时间约为普通高斯粒子滤波时间的75%。可见改进混合高斯滤波，由于其根据系 统的滤波模型特点采用混合的滤波方法，获取了更高的滤波精度，同时滤波时间也有所减 少。
本发明说明书中未作详细描述的内容属于本领域专业技术人员公知的现有技术。
权利要求
一种用于惯性组合导航系统中的改进混合高斯粒子滤波方法，包括下列步骤(1)建立组合导航系统的状态方程、观测方程及噪声模型；(2)建立符合步骤(1)所述噪声模型的高斯粒子滤波算法；其特征在于还包括以下步骤(3)组合导航系统的状态方程为弱非线性的，采用UKF滤波方法获取混合高斯粒子滤波状态更新过程中的高斯分布参数，更新组合导航系统的状态方程；(4)采用步骤(2)所述高斯粒子滤波算法实现组合导航系统的状态更新及其余部分解算；(5)由导航计算机根据步骤(4)更新后的导航系统的状态方程和观测方程，完成组合导航的数据处理及解算。
2.根据权利要求1所述的一种用于惯性组合导航系统中的改进混合高斯粒子滤波方 法，其特征在于步骤(3)所述的高斯分布参数获取方法及步骤如下(a)初始化将过程噪声和量测噪声增广为状态向量，增广后的状态向量为x%相应的 采样点向量为Xa，Pc)为原状态向量协方差初始估计值，Pv是过程噪声方差，Pn是量测噪声方 差，在该状态扩展过程中，把陀螺及加速度的有色噪声按状态扩展法转化为白噪声；(b)计算采样点根据状态量^及其对应的方差厂构造一个n行，2n+l列的矩阵，用定 点采样的方式获取代表状态量的2n+l各Sigma点；(c)时间更过程根据步骤(b)获取的Sigma点和系统方程，利用KF状态更新公式获 取系统的状态预测量毛和量测预测量、w ；(d)量测更新过程根据步骤(c)的到的时间根系预测信息和最新的观测量zk，利用KF 量测更新公式获取状态量的最终滤波状态量毛及其对应的方差Pk ；这样通过UKF获取了更新后状态量的多维高斯分布参数，为粒子滤波提供状态量及噪 声的分布参数；其中，xa表示Sigma点集，Pa为其对应方差；毛/w为状态预测值，‘w为量测预测值；毛 为状态量滤波值，Pk为其对应的方程。
3.根据权利要求1所述的一种用于惯性组合导航系统中的改进混合高斯粒子滤波方 法，其特征在于步骤(4)所述的实现组合导航系统的状态更新及其余部分解算的方法如 下Stepl 初始化选取步骤(3)中合适的高斯分布参数，使高斯粒子服从N(X(l;il(l，E J 正态分布；Step2 时间更新过程求出步骤(3)所述UKF滤波方法更新组合导航系统的状态后的 正态分布参数，使更新后的高斯粒子服从TV(丸；夂，良)正态分布；Step3 量测更新过程根据步骤Step2所述更新后的高斯粒子的分布情况 抽样得到更新后的高斯粒子二 ；计算高斯粒子权值。力=P(yn \xnu))；归 Step4 计算滤波及相关参数根据步骤St印3所述高斯粒子权值跟新状态分布，使量 测更新后的粒子服从N(xn ； u n, E n)正态分布，并得到状态量的滤波值P 及其相应的方差 ；其中凡 St印5 返回步骤(2)；其中 )表示标准多维高斯分布，χ，μ，Σ分别表示对于的状态量、均值和均 方差；Ν(χ。； μ ο, Σ ο)表示状态量的初始分布，Ar(毛;Ai,i )表示状态更新后的分布；{&(7)}义 表示状态量的样本点集，表示其对应的权值ωη(Λ。
全文摘要
本发明公布了一种用于惯性组合导航系统中的改进混合高斯粒子滤波方法，本发明首先根据惯性组合导航的特点建立SINS/GPS组合导航系统的状态方程、观测方程及噪声模型；根据该模型特点，建立符合要求的混合高斯高斯粒子滤波算法结构。根据上述算法结构，把粒子滤波分为两步，采用UKF算法获取混合高斯粒子滤波状态更新过程中的高斯分布参数。采用粒子滤波算法实现状态更新及其余部分算法，并给出混合高斯粒子滤波的实现算法流程。由导航计算机根据惯性组合导航的滤波模型及算法流程，完成组合导航的数据处理及解算工作。本发明一方面提高了组合导航系统的滤波精度，同时滤波时间也有一定的减少，能够较好的解决组合导航系统的非线性滤波问题。
文档编号G01C21/16GK101852615SQ20101017581
公开日2010年10月6日 申请日期2010年5月18日 优先权日2010年5月18日
发明者于明清, 刘建业, 吕品, 周翟和, 曾庆化, 熊剑, 赖际舟, 赵伟 申请人:南京航空航天大学