CN101852615A - 一种用于惯性组合导航系统中的改进混合高斯粒子滤波方法 - Google Patents

Abstract

本发明公布了一种用于惯性组合导航系统中的改进混合高斯粒子滤波方法，本发明首先根据惯性组合导航的特点建立SINS/GPS组合导航系统的状态方程、观测方程及噪声模型；根据该模型特点，建立符合要求的混合高斯高斯粒子滤波算法结构。根据上述算法结构，把粒子滤波分为两步，采用UKF算法获取混合高斯粒子滤波状态更新过程中的高斯分布参数。采用粒子滤波算法实现状态更新及其余部分算法，并给出混合高斯粒子滤波的实现算法流程。由导航计算机根据惯性组合导航的滤波模型及算法流程，完成组合导航的数据处理及解算工作。本发明一方面提高了组合导航系统的滤波精度，同时滤波时间也有一定的减少，能够较好的解决组合导航系统的非线性滤波问题。

Description

一种用于惯性组合导航系统中的改进混合高斯粒子滤波方法

技术领域

本发明是一种用于惯性组合导航系统中新的改进混合高斯粒子滤波方法，属于多传感器信息融合技术，是一种用于多传感器数据融合方法。该专利内容同样适用于惯性组合导航、目标识别与跟踪、图像处理、模式识别等其他多传感器信息融合及多源数据处理应用领域。

背景技术

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

在SINS/GPS深组合导航系统中，当采用导航参数误差量组成状态方程，用GPS伪距构成观测方程时，滤波系统中会出现混合模型。在混合模型中，状态方程可能是线性或弱非线性的，观测方程为一般非线性或强非线性。针对上述混合模型采用单一的滤波方法效果会受到影响，如果采用线性卡尔曼滤波或非线性的EKF或UKF，滤波精度会受到观测方程中强非线性模型的影响。如果采用一般的粒子滤波方法，由于滤波维数高，选取的粒子数目会比较大，计算量大，难以满足组合导航系统的实时型要求。如何在组合导航的混合模型系统中，采用混合滤波方法，把模型和多种数据处理方式有机的结合起来是组合导航系统滤波方法的难点之一，本发明专利是针对上述情况的一种有效组合导航系统数据处理方法。

发明内容

技术问题：在组合导航系统中，当状态方程出现弱非线性，观测方程非线性的情况下，本发明专利提出了一种新的混合滤波方法。该方法的主要思路是在GPF滤波框架下，有机的把UKF和粒子滤波有机的结合起来，在减少粒子滤波的计算量的同时，提高了系统的滤波精度。

本发明为实现上述目的，采用如下技术方案：

本发明一种用于惯性组合导航系统中的改进混合高斯粒子滤波方法，包括下列步骤：

(1)建立组合导航系统的状态方程、观测方程及噪声模型；

(2)建立符合步骤(1)所述噪声模型的高斯粒子滤波算法；

(3)组合导航系统的状态方程为弱非线性的，采用UKF滤波方法获取混合高斯粒子滤波状态更新过程中的高斯分布参数，更新组合导航系统的状态方程；

(4)采用步骤(2)所述高斯粒子滤波算法实现组合导航系统的状态更新及其余部分解算；

(5)由导航计算机根据步骤(4)更新后的导航系统的状态方程和观测方程，完成组合导航的数据处理及解算。

优选地，步骤(3)所述的高斯分布参数获取方法如下：

(a)初始化：将过程噪声和量测噪声增广为状态向量，增广后的状态向量为x^a，相应的采样点向量为χ^a，P₀为原状态向量协方差初始估计值，P_v是过程噪声方差，P_n是量测噪声方差，在该状态扩展过程中，把陀螺及加速度的有色噪声按状态扩展法转化为白噪声；

(b)计算采样点：根据状态量χ^a及其对应的方差P^a构造一个n行，2n+1列的矩阵，用定点采样的方式获取代表状态量的2n+1各Sigma点；

(c)时间更过程：根据步骤(b)获取的Sigma点和系统方程，利用KF状态更新公式获取系统的状态预测量

和量测预测量

(d)量测更新过程：根据步骤(c)的到的时间根系预测信息和最新的观测量z_k，

利用KF量测更新公式获取状态量的最终滤波状态量

及其对应的方差P_k；

这样通过UKF获取了更新后状态量的多维高斯分布参数，为粒子滤波提供状态量及噪声的分布参数；

其中，χ^a表示Sigma点集，P^a为其对应方差；

为状态预测值，

为量测预测值；

为状态量滤波值，P_k为其对应的方程。

优选地，步骤(4)所述的实现组合导航系统的状态更新及其余部分解算的方法如下：

Step1：初始化：选取步骤(3)中合适的高斯分布参数，使高斯粒子服从N(x₀；μ₀，∑₀)正态分布；

Step2：时间更新过程：求出步骤(3)所述UKF滤波方法更新组合导航系统的状态后的正态分布参数，使更新后的高斯粒子服从正态分布；

Step3：量测更新过程：根据步骤Step2所述更新后的高斯粒子的分布情况

抽样得到更新后的高斯粒子计算高斯粒子权值

归一化高斯粒子权值

Step4：计算滤波及相关参数：根据步骤Step3所述高斯粒子权值跟新状态分布，使量测更新后的粒子服从N(x_nz；μ_n，∑_n)正态分布，并得到状态量的滤波值μ_n及其相应的方差∑_n；其中

Step5：返回步骤(2)；

其中，N(x；μ，∑)表示标准多维高斯分布，x，μ，∑分别表示对于的状态量、均值和均方差；N(x₀；μ₀，∑₀)表示状态量的初始分布，

表示状态更新后的分布；

表示状态量的样本点集，表示其对应的权值ω_n ^(j)。

有益效果：

本发明的方法具有如下优点：根据组合导航滤波模型的特点，当状态方程为弱非线性，观测方程为非线性时，把高斯粒子滤波分为两个部分分步实现。首先采用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＝[φ，δv，δL，δλ，δh，b，d]，根据传感器误差模型及惯导误差方程建立状态方程。其中φ＝[φ_e，φ_n，φ_u]为平台误差角，δv＝[δv_e，δv_n，δv_u]表示速度误差，δL，δλ，δh表示位置误差。关于上述状态量的微分方程及其离散化可以参考文献。b，d表示深组合时GPS的时钟误差，其中b表示等效时钟误差，d表示等效时钟频率误差，其对应的微分方程是：

其中状态方程的噪声为：W＝[w_gx，w_gy，w_gz，w_ax，w_ay，w_az，w_b，w_d]。根据上述描述，可以列出系统的状态方程

在该系统中，陀螺及加速度计的有色噪声没有通过状态扩展法处理，是有色噪声，该状态方程具有弱非线性。该有色噪声将在粒子滤波的参数求解和权值更新过程中单独处理，目的是降低系统状态量的维数。

设载体在地球大地坐标系统的位置经度、纬度和高度是λ、L、h，其在地球直角坐标系的对应的位置x、y、z可由(1)式表示。第i颗卫星对应的伪距可由(2)式表示。

$\left\{\begin{array}{c}x=(R_n+h)\cos L\cos \mathrm{\λ}\\ y=(R_n+h)\cos L\sin \mathrm{\λ}\\ z=(R_n(1-e^2)+h)\sin L\end{array}\right.---\left(1\right)$

${\mathrm{\ρ}}_i=\sqrt{{(X_i-x)}^2+{(Y_i-y)}^2{(Z_i-z)}^2}+b+w_g---\left(2\right)$

设系统的观测量Z＝[ρ₁，ρ₂，ρ₃，ρ₄]，把L＝L+δL，λ＝λ+δλ，h＝h+δh代入(1)式，再把(1)式代入(2)式，就可以得到系统的观测方程。根据上述说明，可以列出系统的观测方程Z(t)＝h(X(t))+V(t)。

2)建立符合模型特点的混合高斯粒子滤波算法结构

高斯粒子滤波是一种非线性粒子滤波，能较好的解决非线性滤波问题，为了便于实现混合粒子滤波，需把高斯粒子滤波分解为较为独立的两个部分分步实现。利用UKF获取状态更新后的高斯分布参数，采用高斯粒子滤波实现粒子权值更新，从而获取最终的滤波值。设离散系统后验密度函数的求解可由递推贝叶斯估计公式给出，该过程可总结为如下公式。

p(x_n|y_1：n-1)＝∫p(x_n|x_n-1)p(x_n-1|y_1：n-1)dx_n-1(预测-时间更新)(3)

p(x_n|y_1：n)＝C_np(x_n|t_1：n-1)p(y_n|x_n)(滤波-量测更新)(4)

其中C_n＝(∫p(x_n|y_1：n-1)p(y_n|x_n)dx_n)^-1为归一化因子。假设在时刻n，p(x_n|y_0：n-1)可由(5)式的高斯分布表示

$p\left(x_n\right|y_{1:n-1})=\∫p\left(x_n\right|x_{n-1})p\left(x_{n-1}\right|y_{0:n-1})d{x}_{n-1}\≈N(x_n;{\stackrel{\‾}{\mathrm{\μ}}}_n,{\stackrel{\‾}{\mathrm{\Σ}}}_n)---\left(5\right)$

当得到的n时刻的量测值y_n后，(4)式的滤波密度函数可由(6)式表示，

$p\left(x_n\right|y_{1:n})=C_{n}p\left(x_n\right|y_{1:n-1})p\left(y_n\right|x_n)\≈C_{n}N(x_n;{\stackrel{\‾}{\mathrm{\μ}}}_n,{\stackrel{\‾}{\mathrm{\Σ}}}_n)p\left(y_n\right|x_n)---\left(6\right)$

$\hat{p}\left(x_n\right|y_{1:n})\≈N(x_n;{\mathrm{\μ}}_n,{\mathrm{\Σ}}_n)$

在高斯粒子滤波的实现过程中，上述算法可以总结为两个阶段，首先根据状态方程和相关参数，获取粒子的高斯分布参数，其次首先如果求出该密度函数，滤波问题也得到解决。假设(6)式的滤波密度函数可由(7)式高斯分布来逼近。只要求取(5)和(7)式高斯分布的参数，就可以计算出滤波结果。

对于一般的粒子滤波，无论是状态方程还是量测方程都是非线性的表达形式，描述了系统的一般形式，适合于通用的非线性应用场合。在上述组合导航滤波模型中，系统方程是弱非线性的，只是系统的噪声是非高斯的，观测方程是强非线性的。在这种模型中，采用常用的粒子滤波算法，计算量比较大，难以满足组合导航系统实时性要求。同时由于粒子数目难以满足粒子滤波的基本要求，滤波精度也会受到影响。在这种特殊的系统模型中，本专利针对基本高斯粒子滤波提出了一种改进新算法。改进的主要方法是，利用UKF算法实现高斯粒子滤波的状态更新过程，获取高斯分布的参数，保留高斯粒子滤波中的量测更新及其它计算过程。在传统的高斯粒子滤波状态更新过程中，需要通过状态方程及其噪声分布对每个粒子分别进行状态更新，而改进算法在状态更新的过程中，只需采用UKF对状态参数进行更新。在新算法中，由于UKF只需较少的sigma点就能获取精度较高的状态更新参数，所以算法的滤波精度也有一定的提高，同时计算量有明显的减少。

3)采用UKF算法获取GPF状态更新过程中的高斯分布参数

从改进混合高斯粒子滤波算法的结构上看，改进混合高斯粒子滤波的实现分为两步，时间更新和量测更新。在时间更新部分，根据系统的状态方程和噪声分布情况，获取根系后粒子的分布参数。在量测更新部分，根据粒子滤波算法修改粒子的权值，从而得到更加接近后验概率分布的分布参数，获取最终的滤波结果。针对组合导航系统中状态方程弱非线性的情况，本专利提出了采用UKF获取量测更新过程中的高斯分布参数。其具体实现方法如下：

UKF滤波器分为以下几个步骤

a)初始化

将过程噪声和量测噪声增广为状态向量，增广后的状态向量为x^a，相应的采样点向量为χ^a，P₀为原状态向量协方差初始估计值，P_v是过程噪声方差，P_n是量测噪声方差。

x^a＝[x^T v^T η^T]^T                          (8)

χ^a＝[(χ^x)^T  (χ^v)^T  (χ^η)^T]^T    (9)

${\hat{x}}_0^a=E\left[x^a\right]---\left(10\right)$

$P_0=E\left[(x_0-{\hat{x}}_0){(x_0-{\hat{x}}_0)}^T\right]---\left(11\right)$

$P_0^a=E\left[(x_0^a-{\hat{x}}_0^a){(x_0^a-{\hat{x}}_0^a)}^T\right]=\left[\begin{array}{ccc}{P}_0& 0& 0\\ 0& P_v& 0\\ 0& 0& P_n\end{array}\right]---\left(12\right)$

在该状态扩展过程中，把陀螺及加速度的有色噪声按状态扩展法转化为白噪声处理，由于观测噪声在系统方程的建立过程中已经扩展在状态量里，这里无须额外处理。

b)计算采样点

构造一个n行(n为增广状态向量的维数)，2n+1列的矩阵，各列形式如下

${x_{0,}^a}_{k-1}={\hat{x}}_{k-1}^a---\left(13\right)$

${\mathrm{\χ}}_{i,k-1}^a={\hat{x}}_{k-1}^a+{\left(\sqrt{(n+\mathrm{\λ})P_{k-1}^a}\right)}_i$ i＝1，...，n                    (14)

${\mathrm{\χ}}_{i,k-1}^a={\hat{x}}_{k-1}^a-{\left(\sqrt{(n+\mathrm{\λ})P_{k-1}^a}\right)}_{i-n}$ i＝n+1，...，2n                 (15)

这里，λ＝α²(n+k)-n，α决定采样点距均值的远近程度，通常被赋一个较小的正值，k≥0保证方差阵的半正定性。

c)时间更新方程

${\mathrm{\χ}}_{i,k/k-1}^x=f({\mathrm{\χ}}_{i,k/k-1}^x,{\mathrm{\χ}}_{i,k-1}^v)$ (i＝0，1，...，2n)            (16)

上式中f为系统的非线性状态方程式。

${\hat{x}}_{k/k-1}=\underset{i=0}{\overset{2n}{\mathrm{\Σ}}}{W_i}^{\left(m\right)}{\mathrm{\χ}}_{i,k/k-1}^x---\left(17\right)$

$P_{k/k-1}=\underset{i=0}{\overset{2n}{\mathrm{\Σ}}}{W_i}^{\left(c\right)}[{\mathrm{\χ}}_{i,k/k-1}^x-{\hat{x}}_{k/k-1}]{[{\mathrm{\χ}}_{i,k/k-1}^x-{\hat{x}}_{k/k-1}]}^T--\left(18\right)$

$z_{k/k-1}=h({\mathrm{\χ}}_{k/k-1}^x,{\mathrm{\χ}}_{k-1}^{\mathrm{\η}})---\left(19\right)$

h为系统的非线性量测方程式。

${\hat{z}}_{k/k-1}=\underset{i=0}{\overset{2n}{\mathrm{\Σ}}}{W_i}^{\left(m\right)}{z}_{i,k/k-1}---\left(20\right)$

其中

${W_0}^{\left(m\right)}=\frac{\mathrm{\λ}}{n+\mathrm{\λ}},{W_0}^{\left(c\right)}=\frac{\mathrm{\λ}}{n+\mathrm{\λ}}+(1-{\mathrm{\α}}^2+\mathrm{\β})---\left(21\right)$

${W_i}^{\left(m\right)}={W_i}^{\left(c\right)}=\frac{1}{2(n+\mathrm{\λ})},i=\mathrm{1,2},...2n---\left(22\right)$

β用于包含状态量分布的高阶成分信息，β≥0。

d)量测更新方程

$P_{{\hat{z}}_k{\hat{z}}_k}=\underset{i=0}{\overset{2n}{\mathrm{\Σ}}}{W}_i^{\left(c\right)}[z_{i,k/k-1}-{\hat{z}}_{k/k-1}]{[z_{i,k/k-1}-{\hat{z}}_{k/k-1}]}^T---\left(23\right)$

$P_{x_k{z}_k}=\underset{i=0}{\overset{2n}{\mathrm{\Σ}}}{W}_i^{\left(c\right)}[{\mathrm{\χ}}_{i,k/k-1}^x-{\hat{x}}_{k/k-1}]{[z_{i,k/k-1}-{\hat{z}}_{k/k-1}]}^T---\left(24\right)$

$\mathrm{\κ}=P_{x_k{z}_k}{P}_{{\hat{z}}_k{\hat{z}}_k}^{-1}---\left(25\right)$

${\hat{x}}_k={\hat{x}}_{k/k-1}+\mathrm{\κ}(z_k-{\hat{z}}_{k/k-1})---\left(26\right)$

$P_k=P_{k/k-1}-\mathrm{\κ}{P}_{{\hat{z}}_k{\hat{z}}_k}{\mathrm{\κ}}^T---\left(27\right)$

通过UKF获取了更新后状态量的多维高斯分布参数，其中x^T的均值及对应的方差代表了导航参数X＝[φ，δv，δL，δλ，δh，b，d]的状态分布参数。同时可以获取噪声的相关参数及分布，为粒子滤波提供状态量及噪声的分布参数。

4)采用粒子滤波算法原理和改进措施设计出符合要求的混合高斯粒子滤波算法流程

通过上述UKF滤波方法获取了导航参数状态更新后的状态量及噪声分布参数，利用这些参数，进行粒子抽样和权值更新并最终获取滤波值。算法的主要思想是利用UKF算法实现高斯粒子滤波的状态更新过程，获取高斯分布的参数，保留高斯粒子滤波中的量测更新及其它计算过程。其中状态量的多维高斯分布参数已经通过UKF的方法获取，粒子的抽样可以根据状态量更新后的分布参数

来获取，只是需要注意对有色噪声按噪声模型进行还原处理。在粒子抽样完成后，更加粒子滤波权值更新公式求得更新后的粒子权值，根据更新的粒子及权值从而得到状态量的滤波值。由于改进算法在状态更新的过程中，只需要对其高斯分布参数及均值和方差进行更新，这样计算量就大大降低了。在新算法中，由于UKF只需较少的sigma点就能获取精度较高的状态更新参数，所以算法的滤波精度也有一定的提高，同时，计算量有明显的减少。通过上述说明可以给出该算法的实现流程。如下：

Step1：初始化：选取合适的参数，使粒子服从N(x₀；μ₀，∑₀)正态分布；

Step2：时间更新过程：根据上次滤波结果系统的状态方程，利用UKF滤波方法获取更新后状态量的分布参数，状态更新公式分别求出状态更新后的正态分布参数，使更新后的粒子服从

正态分布。

Step3：量测更新过程：根据更新后粒子的分布情况

抽样得到更新后的粒子，表示为

根据公式

计算粒子权值；根据公式归一化权值。

Step4：计算滤波及相关参数：根据粒子及相应权值计算滤波结果和相关参数，使量测更新后的粒子服从N(x_n；μ_n，∑_n)正态分布，并得到状态量的滤波值μ_n及其相应的方差∑_n；其中

Step5：从第(2)步重复上述步骤，循环以上步骤。

从上叙流程可以看出，首先对改进混合高斯粒子滤波进行初始化，使得状态量的初始值为导航系统的初始对准值。其次要进行粒子抽样，抽样的方法是以UKF滤波后的均值为粒子的均值，根据噪声模型产生粒子附加噪声，然后根据状态方程模型获取粒子。

5)完成组合导航的数据处理及解算工作

由导航计算机根据组合导航系统的状态方程和观测方程，IMU的输出及GPS的数据完成组合导航的数据融合工作，并完成组合导航数据处理解算。

导航系统的初值由初始对准的结果给出，航迹及各传感器的数据输出有相应的模拟器给出，根据导航系统的解算原理及改进混合高斯滤波方法解算出导航参数的滤波结果。其中改进混合高斯粒子滤波算法把高斯粒子滤波分为两步，当组合导航系统的状态方程为弱非线性是，采用UKF滤波算法获取系统状态量的多维高斯分布参数。得到新的观测量后粒子高斯粒子滤波技术获取更新后的权值，从而得到导航参数的滤波值。改进混合高斯粒子滤波在上述模型下，提高了滤波精度，同时滤波时间也有一定的减少。在同等条件下将改进混合高斯粒子滤波算法与普通高斯粒子滤波算法进行组合导航系统数据处理的对比。

图2、3、4所示为普通高斯粒子滤波算法和基于UKF的改进高斯粒子滤波算法在组合导航数据处理中的应用结果比较，其中①为普通高斯粒子滤波器用于组合导航系统的滤波结果，②为改进混合高斯粒子滤波在组合导航系统中的滤波结果。由图2、3、4可以看出，改进混合普通粒子滤波算法在滤波精度上有一定的优势，其位置误差为11.5米，与普通高斯粒子滤波的15.2米相比，滤波精度有了一定的提高。图5给出了两种滤波方式在不同粒子数目下的平均单步滤波计算时间，其中①为普通高斯粒子滤波器的滤波时间，②为改进混合高斯粒子滤波的滤波时间。由图可以看出改进混合高斯粒子滤波的滤波时间有所减少，其滤波时间约为普通高斯粒子滤波时间的75％。可见改进混合高斯滤波，由于其根据系统的滤波模型特点采用混合的滤波方法，获取了更高的滤波精度，同时滤波时间也有所减少。

本发明说明书中未作详细描述的内容属于本领域专业技术人员公知的现有技术。

Claims (3)

1.一种用于惯性组合导航系统中的改进混合高斯粒子滤波方法，包括下列步骤：

(1)建立组合导航系统的状态方程、观测方程及噪声模型；

(2)建立符合步骤(1)所述噪声模型的高斯粒子滤波算法；

其特征在于还包括以下步骤：

(3)组合导航系统的状态方程为弱非线性的，采用UKF滤波方法获取混合高斯粒子滤波状态更新过程中的高斯分布参数，更新组合导航系统的状态方程；

(4)采用步骤(2)所述高斯粒子滤波算法实现组合导航系统的状态更新及其余部分解算；

(5)由导航计算机根据步骤(4)更新后的导航系统的状态方程和观测方程，完成组合导航的数据处理及解算。

2.根据权利要求1所述的一种用于惯性组合导航系统中的改进混合高斯粒子滤波方法，其特征在于步骤(3)所述的高斯分布参数获取方法及步骤如下：

(a)初始化：将过程噪声和量测噪声增广为状态向量，增广后的状态向量为x^a，相应的采样点向量为x^a，P₀为原状态向量协方差初始估计值，P_v是过程噪声方差，P_n是量测噪声方差，在该状态扩展过程中，把陀螺及加速度的有色噪声按状态扩展法转化为白噪声；

(b)计算采样点：根据状态量x^a及其对应的方差P^a构造一个n行，2n+1列的矩阵，用定点采样的方式获取代表状态量的2n+1各Sigma点；

(c)时间更过程：根据步骤(b)获取的Sigma点和系统方程，利用KF状态更新公式获取系统的状态预测量

和量测预测量

(d)量测更新过程：根据步骤(c)的到的时间根系预测信息和最新的观测量z_k，利用KF量测更新公式获取状态量的最终滤波状态量

及其对应的方差P_k；

这样通过UKF获取了更新后状态量的多维高斯分布参数，为粒子滤波提供状态量及噪声的分布参数；

其中，x^a表示Sigma点集，P^a为其对应方差；

为状态预测值，

为量测预测值；为状态量滤波值，P_k为其对应的方程。

3.根据权利要求1所述的一种用于惯性组合导航系统中的改进混合高斯粒子滤波方法，其特征在于步骤(4)所述的实现组合导航系统的状态更新及其余部分解算的方法如下：

Step1：初始化：选取步骤(3)中合适的高斯分布参数，使高斯粒子服从N(x₀；μ₀，∑₀)正态分布；

Step2：时间更新过程：求出步骤(3)所述UKF滤波方法更新组合导航系统的状态后的正态分布参数，使更新后的高斯粒子服从

正态分布；

Step3：量测更新过程：根据步骤Step2所述更新后的高斯粒子的分布情况

抽样得到更新后的高斯粒子

计算高斯粒子权值

归一化高斯粒子权值

Step4：计算滤波及相关参数：根据步骤Step3所述高斯粒子权值跟新状态分布，使量测更新后的粒子服从N(x_n；μ_n，∑_n)正态分布，并得到状态量的滤波值μ_n及其相应的方差∑_n；其中

Step5：返回步骤(2)；

其中，N(x；μ，∑)表示标准多维高斯分布，x，μ，∑分别表示对于的状态量、均值和均方差；N(x₀；μ₀，∑₀)表示状态量的初始分布，

表示状态更新后的分布；

表示状态量的样本点集，表示其对应的权值ω_n ^(j)。

Images