常规卡尔曼滤波器(KF)是GNSS/SINS松组合导航系统中常用的信息融合方法，其以最小均方误差(MMSE)准则为基础，在高斯假设下是最优的^[1-2]。但在实际应用中，由于内外因素的影响，惯性传感器存在较大的动态误差，使得系统的状态模型不准确。当受到电磁干扰或气候复杂多变时，GNSS的测量噪声模型性能下降，且多表现为重尾脉冲噪声干扰^[3-4]。上述情况均导致系统噪声协方差矩阵及测量噪声协方差矩阵不再满足高斯假设，且无法准确描述，进而导致KF不再满足高斯假设，其估计结果不再保持最优。主要原因是基于MMSE准则的KF通常以方差、协方差等二阶统计量作为优化代价，所以对较大的异常值较敏感，导致KF在非高斯噪声环境中的鲁棒性下降。

信息学习理论中的相关熵可以捕获随机变量的高阶统计量，是一种局部相似性度量，因此对于异常值不敏感^[5]。最大熵准则以相关熵为基础，不仅可以捕获误差变量的方差等二阶统计量，还可以保留其高阶统计量，提高系统的鲁棒性。Izanloo等^[6]将该方法应用于舰船导航，仿真结果证明了其有效性。Chen等^[7]将该方法应用于简单的目标跟踪模型，在重尾噪声干扰环境下证明了其优越性，且其性能优于粒子滤波(PF)。Liu等^[8]将该方法应用于空间航行器相对姿态测量，取得较好的效果。

为提高GNSS/SINS组合导航系统在复杂环境下的滤波精度，本文提出最大熵滤波方法，并设计其迭代流程及迭代停止条件。该法保留了KF的状态均值的传播过程和协方差矩阵的传播过程，具有递归结构。

1 组合导航系统模型

选取东北天坐标系作为导航坐标系，系统状态量选取为SINS解算的导航参数误差量，则GNSS/INS松组合系统的状态方程为^[9-10]：

$ \boldsymbol{X}_k=\boldsymbol{\Phi}_{k \mid k-1} \boldsymbol{X}_{k-1}+\boldsymbol{\Gamma}_k \boldsymbol{W}_k $

(1)

式中，X_k为k时刻的n维状态向量，Φ_k|k－1为k－1时刻到k时刻的状态转移矩阵，Γ_k为系统噪声分配矩阵，W_k为协方差矩阵为Q_k的系统噪声向量。X_k可表示为：

$ \boldsymbol{X}_k=\left[\begin{array}{lllll} \varphi & \delta v & \delta p & \varepsilon^b & \nabla^b \end{array}\right]^{\mathrm{T}} $

(2)

式中，$ \varphi=\left[\begin{array}{lll} \varphi_E & \varphi_N & \varphi_U \end{array}\right]$为SINS平台角误差信息，$ \delta v=\left[\begin{array}{lll} \delta v_E & \delta v_N & \delta v_U \end{array}\right]$为SINS速度误差信息，$ \delta p=\left[\begin{array}{lll} \delta \lambda & \delta L & \delta h \end{array}\right]$为SINS位置误差信息，$ \varepsilon^b 、\nabla^b$分别为陀螺、加速度计三轴误差的一阶马尔可夫过程。

测量方程可以表示为：

$ \boldsymbol{Z}_k=\boldsymbol{H}_k \boldsymbol{X}_k+\boldsymbol{V}_k $

(3)

式中，$ \boldsymbol{Z}_k$为m维测量向量，是GNSS与SINS位置及速度输出的差值，H_k为测量矩阵，V_k为协方差矩阵为R_k的测量噪声向量。

2 基于最大熵准则的滤波算法 2.1 相关熵

相关熵是两个随机变量之间的相似性度量。假设两个随机变量X和Y的联合分布函数为$ F_{X Y}(x, y)$则其相关熵定义为：

$ \begin{gathered} V(X, Y)=E[\kappa(X, Y)]= \\ \int \kappa(x, x) \cdot \mathrm{d} F_{X Y}(x, y) \end{gathered} $

(4)

式中，E[·]为求期望运算，$ \kappa(\bullet, \bullet)$为核函数。本文选取的核函数为高斯核函数：

$ \kappa(x, y)=G_\sigma(e)=\exp \left(-\frac{e^2}{2 \sigma^2}\right) $

(5)

式中，$ e=x-y, \sigma$为核宽度。

然而，在大多情况下，联合分布$ F_{X Y}(x, y)$是未知的。此时，可以使用样本值来估计相关熵：

$ \hat{V}(X, Y)=\frac{1}{N} \sum\limits_{i=1}^N G_\sigma(e(i)) $

(6)

式中，$ e(i)=x(i)-y(i)$。

对高斯核函数进行泰勒级数展开：

$ V(X, Y)=\sum\limits_{n=0}^{\infty} \frac{(-1)^n}{2^n \cdot \sigma^{2 n} \cdot n!} E\left[(X-Y)^{2 n}\right] $

(7)

由式(7)可以看出，相关熵是随机变量的所有偶阶矩的加权和。当核宽度σ非常大时，相关熵主要取决于二阶矩。

2.2 基于最大熵准则的卡尔曼滤波器

KF在高斯噪声下可获得最优的估计性能，但是在非高斯噪声下，其性能显著下降。主要原因是KF是建立在MMSE准则下的，该准则仅捕获信号的二阶统计量，对较大的异常信息较为敏感。根据式(7)，信号的相关熵包含了误差的二阶和更高阶矩，因此基于最大熵准则(MCC)下的KF能在非高斯噪声环境下表现出优异的性能。

设$ \hat{\boldsymbol{X}}_k$为状态X_k的卡尔曼估值，P_k为估值$ \hat{\boldsymbol{X}}_k$的均方误差矩阵，$ \hat{\boldsymbol{X}}_{k \mid k-1}$为X_k的一步预测值，$ \boldsymbol{P}_{k \mid k-1}$为X_k的一步预测均方误差矩阵，则有：

$ \left[\begin{array}{c} \hat{\boldsymbol{X}}_{k \mid k-1} \\ \boldsymbol{Z}_k \end{array}\right]=\left[\begin{array}{c} \boldsymbol{I} \\ \boldsymbol{H}_k \end{array}\right] \boldsymbol{X}_k+\boldsymbol{F}_k $

(8)

式中，I为n×n单位矩阵，F_k可表示为：

$ \boldsymbol{F}_k=\left[\begin{array}{c} \hat{\boldsymbol{X}}_{k \mid k-1}-\boldsymbol{X}_k \\ \boldsymbol{V}_k \end{array}\right] $

(9)

且有：

$ \begin{gathered} E\left[\boldsymbol{F}_k \boldsymbol{F}_k^{\mathrm{T}}\right]=\left[\begin{array}{cc} \boldsymbol{P}_{k \mid k-1} & 0 \\ 0 & \boldsymbol{R}_k \end{array}\right]= \\ {\left[\begin{array}{cc} \boldsymbol{B}_{P, k \mid k-1} \boldsymbol{B}_{P, k \mid k-1}^{\mathrm{T}} & 0 \\ 0 & \boldsymbol{B}_{V, k} \boldsymbol{B}_{V, k}^{\mathrm{T}} \end{array}\right]=\boldsymbol{B}_k \boldsymbol{B}_k^{\mathrm{T}}} \end{gathered} $

(10)

式中，B_k可以由$ E\left[\boldsymbol{F}_k \boldsymbol{F}_k^{\mathrm{T}}\right]$的Cholesky分解得到。式(8)两边同时乘以B_k^-1，得到：

$ \boldsymbol{D}_k=\boldsymbol{M}_k \boldsymbol{X}_k+\boldsymbol{E}_k $

(11)

式中$ \boldsymbol{D}_k=\boldsymbol{B}_k^{-1}\left[\begin{array}{c} \hat{\boldsymbol{X}}_{k \mid k-1} \\ \boldsymbol{Z}_k \end{array}\right], \boldsymbol{M}_k=\boldsymbol{B}_k^{-1}\left[\begin{array}{c} \boldsymbol{I} \\ \boldsymbol{H}_k \end{array}\right], \boldsymbol{E}_k= \boldsymbol{B}_k^{-1} \boldsymbol{F}_k$。$ E\left[\boldsymbol{E}_k \boldsymbol{E}_k^{\mathrm{T}}\right]=\boldsymbol{I}, \boldsymbol{E}_k$为白噪声。式(11)可写为：

$ \boldsymbol{E}_k=\boldsymbol{D}_k-\boldsymbol{M}_k \boldsymbol{X}_k $

(12)

设e_{i, k}和d_{i, k}分别为E_k和D_k的第i个元素，M_{i, k}为M_k的第i行。则有：

$ e_{i, k}=d_{i, k}-\boldsymbol{M}_{i, k} \boldsymbol{X}_k $

(13)

建立基于MCC准则的代价函数^[11]：

$ J\left(\boldsymbol{X}_k\right)=\frac{1}{L} \sum\limits_{i=1}^L G_\sigma\left(d_{i, k}-\boldsymbol{M}_{i, k} \boldsymbol{X}_k\right) $

(14)

X_k的最优估计为：

$ \hat{\boldsymbol{X}}_k=\underset{X_k}{\operatorname{argmax}} J\left(\boldsymbol{X}_k\right)=\underset{X_k}{\operatorname{argmax}} \sum\limits_{i=1}^L G_\sigma\left(e_{i, k}\right) $

(15)

式中，L=m+n。

通过式(16)得到最优解X_k：

$ \frac{\partial J\left(\boldsymbol{X}_k\right)}{\partial \boldsymbol{X}_k}=\sum\limits_{i=1}^L\left[G_\sigma\left(e_{i, k}\right) \boldsymbol{M}_{i, k}^{\mathrm{T}}\left(d_{i, k}-\boldsymbol{M}_{i, k} \boldsymbol{X}_k\right)\right]=0 $

(16)

$ \begin{aligned} \boldsymbol{X}_k= & \left(\sum\limits_{i=1}^L\left[G_\sigma\left(e_{i, k}\right) \boldsymbol{M}_{i, k}^{\mathrm{T}} \boldsymbol{M}_{i, k}\right]\right)^{-1} \cdot \\ & \left(\sum\limits_{i=1}^L\left[G_\sigma\left(e_{i, k}\right) \boldsymbol{M}_{i, k}^{\mathrm{T}} d_{i, k}\right]\right) \end{aligned} $

(17)

式(17)也可以表示为^[12]：

$ \boldsymbol{X}_k=\left(\boldsymbol{M}_k^{\mathrm{T}} \boldsymbol{C}_k \boldsymbol{M}_k\right)^{-1}\left(\boldsymbol{M}_k^{\mathrm{T}} \boldsymbol{C}_k \boldsymbol{D}_k\right) $

(18)

式中，$ \boldsymbol{C}_k=\left[\begin{array}{cc} \boldsymbol{C}_{X, k} & 0 \\ 0 & \boldsymbol{C}_{Z, k} \end{array}\right], \boldsymbol{C}_{X, k}=\operatorname{diag}\left(G_\sigma\left(e_{1, k}\right) \quad \cdots\right.$ $ \left.G_\sigma\left(e_{n, k}\right)\right), \boldsymbol{C}_{Z, k}=\operatorname{diag}\left(G_\sigma\left(e_{n+1, k}\right) \quad \cdots \quad G_\sigma\left(e_{n+m, k}\right)\right)$。

根据矩阵逆引理知识，结合D_k、M_k和C_k的定义，式(18)可写为：

$ \boldsymbol{X}_k=\hat{\boldsymbol{X}}_{k \mid k-1}+\overline{\boldsymbol{K}}_k\left(\boldsymbol{Z}_k-\boldsymbol{H}_k \hat{\boldsymbol{X}}_{k \mid k-1}\right) $

(19)

式中，

$ \left\{\begin{array}{l} \overline{\boldsymbol{K}}_k=\overline{\boldsymbol{P}}_{k \mid k-1} \boldsymbol{H}_k^{\mathrm{T}}\left(\boldsymbol{H}_k \overline{\boldsymbol{P}}_{k \mid k-1} \boldsymbol{H}_k^{\mathrm{T}}+\overline{\boldsymbol{R}}_k\right) \\ \overline{\boldsymbol{P}}_{k \mid k-1}=\boldsymbol{B}_{P, k \mid k-1} \boldsymbol{C}_{\mathrm{X}, k}^{-1} \boldsymbol{B}_{P, k \mid k-1}^{\mathrm{T}} \\ \overline{\boldsymbol{R}}_k=\boldsymbol{B}_{V, k \mid k-1} \boldsymbol{C}_{Z, k}^{-1} \boldsymbol{B}_{V, k \mid k-1}^{\mathrm{T}} \end{array}\right. $

(20)

可以发现，式(18)中X_k依赖于$ \overline{\boldsymbol{K}}_k$；根据式(20)，$ \overline{\boldsymbol{K}}_k$依赖于$ \overline{\boldsymbol{P}}_{k \mid k-1}$和$ \overline{\boldsymbol{R}}_k$；根据式(12)和式(20)，$ \overline{\boldsymbol{P}}_{k \mid k-1}$和$ \overline{\boldsymbol{R}}_k$分别通过$ \boldsymbol{C}_{X, k}$和$ \boldsymbol{C}_{Z, k}$与X_k相关。所以方程(19)是X_k的不动点方程，故计算X_k的最优估计值$ \hat{\boldsymbol{X}}_k$是一个迭代过程。

根据以上分析，基于MCC的卡尔曼滤波算法步骤如下：

1) 选择合适的核带宽σ和较小的正数ε，设置初始估计$ \hat{\boldsymbol{X}}_0$和初始协方差矩阵$ \boldsymbol{P}_0$。

2) 应用式(21)、(22)计算得到$ \hat{\boldsymbol{X}}_{k \mid k-1}$和$ \boldsymbol{P}_{k \mid k-1}$并利用Cholesky分解得到B_k：

$ \hat{\boldsymbol{X}}_{k \mid k-1}=\boldsymbol{\Phi}_{k \mid k-1} \hat{\boldsymbol{X}}_k $

(21)

$ \boldsymbol{P}_{k \mid k-1}=\boldsymbol{\Phi}_{k \mid k-1} \boldsymbol{P}_{k-1} \boldsymbol{\Phi}_{k \mid k-1}^{\mathrm{T}}+\boldsymbol{\Gamma}_{k-1} \boldsymbol{Q}_{k-1} \boldsymbol{\Gamma}_{k-1}^{\mathrm{T}} $

(22)

3) 设i=0，且$ \hat{\boldsymbol{X}}_k^{(0)}=\hat{\boldsymbol{X}}_{k \mid k-1}$，i为迭代次数$ \hat{\boldsymbol{X}}_k^{(i)}$为X_k在k时刻第i次迭代的估计值。

4) 利用式(23)~(29)计算$ \hat{\boldsymbol{X}}_k$：

$ \hat{\boldsymbol{X}}_k^{(i)}=\hat{\boldsymbol{X}}_{k \mid k-1}+\overline{\boldsymbol{K}}_k\left(\boldsymbol{Z}_k-\boldsymbol{H}_k \hat{\boldsymbol{X}}_{k \mid k-1}\right) $

(23)

$ \overline{\boldsymbol{K}}_k=\overline{\boldsymbol{P}}_{k \mid k-1} \boldsymbol{H}_k^{\mathrm{T}}\left(\boldsymbol{H}_k \overline{\boldsymbol{P}}_{k \mid k-1} \boldsymbol{H}_k^{\mathrm{T}}+\overline{\boldsymbol{R}}_k\right) $

(24)

$ \overline{\boldsymbol{P}}_{k \mid k-1}=\boldsymbol{B}_{P, k \mid k-1} \overline{\boldsymbol{C}}_{X, k}^{-1} \boldsymbol{B}_{P, k \mid k-1}^{\mathrm{T}} $

(25)

$ \overline{\boldsymbol{R}}_k=\boldsymbol{B}_{V, k \mid k-1} \overline{\boldsymbol{C}}_{Z, k}^{-1} \boldsymbol{B}_{V, k \mid k-1}^{\mathrm{T}} $

(26)

$ \overline{\boldsymbol{C}}_{X, k}=\operatorname{diag}\left(G_\sigma\left(\bar{e}_{1, k}\right) \quad \cdots \quad G_\sigma\left(\bar{e}_{n, k}\right)\right) $

(27)

$ \overline{\boldsymbol{C}}_{Z, k}=\operatorname{diag}\left(G_\sigma\left(\bar{e}_{n+1, k}\right) \quad \cdots \quad G_\sigma\left(\bar{e}_{n+m, k}\right)\right) $

(28)

$ \bar{e}_{i, k}=d_{i, k}-\boldsymbol{M}_{i, k} \hat{\boldsymbol{X}}_k^{(t-1)} $

(29)

5) 比较当前时刻与前一时刻估计值，以确定迭代停止条件。根据式(2)，导航参数误差的估计值主要有位置误差估计值和速度误差估计值。其中，位置误差中经度误差和纬度误差以rad表示，高度误差以m表示，速度误差均以m/s表示。迭代停止条件可设定为如下两种^[13-14]：

$ \frac{\left\|\hat{\boldsymbol{X}}_k^{(i)}(7: 8)-\hat{\boldsymbol{X}}_k^{(i-1)}(7: 8)\right\|}{\left\|\hat{\boldsymbol{X}}_k^{(i-1)}(7: 8)\right\|} \leqslant \varepsilon $

(30)

$ \frac{\left\|\hat{\boldsymbol{X}}_k^{(i)}(4: 6)-\hat{\boldsymbol{X}}_k^{(i-1)}(4: 6)\right\|}{\left\|\hat{\boldsymbol{X}}_k^{(i-1)}(4: 6)\right\|} \leqslant \varepsilon $

(31)

式(30)是根据水平位置误差估值确定的迭代停止条件，式(31)是根据速度误差估值确定的迭代停止条件。

6) 更新后验协方差矩阵，即估值$ \hat{\boldsymbol{X}}_k$的均方误差矩阵P_k：

$ \begin{gathered} \boldsymbol{P}_k=\left(\boldsymbol{I}-\overline{\boldsymbol{K}}_k \boldsymbol{H}_k\right) \overline{\boldsymbol{P}}_{k \mid k-1}\left(\boldsymbol{I}-\overline{\boldsymbol{K}}_k \boldsymbol{H}_k\right)^{\mathrm{T}}+ \\ \overline{\boldsymbol{K}}_k \overline{\boldsymbol{R}}_k \overline{\boldsymbol{K}}_k^{\mathrm{T}} \end{gathered} $

(32)

3 仿真与分析 3.1 仿真条件

1) 以飞行器为仿真对象，初始位置为118°E、29°N、高度50 m；初始水平姿态角为0°，初始航向角为90°；飞行过程包含爬升、转弯、加速、减速及俯冲等机动过程，时间为3 600 s。

2) 导航信息的初始误差：水平姿态角误差及航向角误差均为0.5°，位置误差为20 m，速度误差为0.6 m/s。

3) 陀螺仪零偏为4°/h，角度随机游走误差为0.4°/$ \sqrt{\mathrm{h}}$；加速度计零偏为50 μg，随机游走误差为5 μg/$ \sqrt{\mathrm{Hz}}$；GNSS三维位置误差均为8 m，三维速度误差均为0.2 m/s。

4) GNSS周期为1 s，SINS周期为0.02 s，滤波周期为1 s。

3.2 混合高斯噪声时的仿真与分析

根据§3.1的设置，正常情况下GNSS的量测噪声满足$ \boldsymbol{V}_k \sim N\left(0, \boldsymbol{R}_k\right)$的高斯分布，其中R_k=diag(8², 8², 8², 0.2², 0.2², 0.2²)。同样，假设Q_k为正常情况下的系统噪声协方差矩阵。

假设测量噪声及系统噪声满足高斯混合分布，即没有精确的系统噪声协方差矩阵及测量噪声协方差矩阵：

$ \boldsymbol{V}_k \sim\left\{\begin{array}{l} N\left(0, \boldsymbol{R}_k\right), 90 \% \\ N\left(0, 100 \boldsymbol{R}_k\right), 10 \% \end{array}\right. $

(33)

$ \boldsymbol{W}_k \sim\left\{\begin{array}{l} N\left(0, \boldsymbol{Q}_k\right), 90 \% \\ N\left(0, 100 \boldsymbol{Q}_k\right), 10 \% \end{array}\right. $

(34)

基于相同的仿真原始数据，利用式(30)所示迭代停止条件，分别使用KF和MCKF(σ=3、ε=10^－9)得到各导航参数误差，如图 1所示。

为了更加直观地分析在混合高斯噪声环境下基于KF和MCKF的滤波精度，统计各导航参数的误差RMS(表 1)。可以看出，在混合高斯噪声环境下，KF的滤波精度明显优于MCKF，且达到了最好性能。主要原因为：两个服从高斯分布的随机变量之和依然满足高斯分布，即KF依然满足最优滤波的基本条件。当核带宽越小时，MCKF的滤波性能越差；反之，随着核带宽增大，其性能逐渐接近于KF。

3.3 重尾脉冲噪声时的仿真与分析

依然假设系统噪声满足式(34)的高斯混合分布，而测量值受到重尾脉冲噪声的干扰，并且假设该重尾噪声满足如下分布：

$ \boldsymbol{V}_k \sim\left\{\begin{array}{l} N\left(0, \boldsymbol{R}_k\right), 90 \% \\ \chi^2\left(0, 100 \boldsymbol{R}_k\right), 10 \% \end{array}\right. $

(35)

式中，χ²为卡方分布。

同样，基于相同的仿真原始数据，并利用式(30)所示迭代停止条件，分别使用KF和MCKF得到各导航参数误差，如图 2所示。图 3为基于KF和MCKF(σ=3、ε=10^－9)的绝对位置误差曲线。

由图 2和3可以看出，在重尾脉冲噪声干扰下，基于MCKF的滤波精度明显更优。

为了更加全面地评价MCKF的滤波性能，给出基于不同核带宽下各导航参数误差的RMS比较及平均循环次数(表 2)。可以看出，在重尾脉冲噪声干扰下，当核带宽介于2~7时，MCKF的滤波精度均优于KF；核带宽过小或过大时，系统的滤波精度不佳；当核带宽逐渐变大并趋于无穷时，根据式(5)、(25)~(28)，MCKF将与KF等价；核带宽越小则平均循环次数越大。

当核带宽不变而迭代阈值不同时，给出各导航参数误差的RMS比较及平均循环次数(表 3)。可以看出，较小的迭代阈值得到的RMS较低，但需要较大的平均迭代次数；与核带宽相比，迭代阈值对导航精度的影响不显著。

在实验中发现，当核带宽较小时(小于2)，滤波收敛性难以保证。主要因为核带宽越小，根据式(25)、(26)计算得到的$ \overline{\boldsymbol{P}}_{k \mid k-1}, \overline{\boldsymbol{R}}_k$的值越大，当超越其临界值时，将会导致滤波发散。当迭代停止条件选择式(31)时，得到的滤波结果与图 1~3相似。

由于MCKF的滤波过程涉及到循环迭代处理过程，所以为了考察MCKF的实时性，在Intel Core i3-1125G4(2.0 GHz/L3 8M) 处理器环境下记录MCKF的每次滤波所耗时间(图 4)。本文所采用的滤波周期为1 s，即共有3 600次滤波；IMU的计算周期为0.02 s。从图 4看出，最大滤波耗时为7×10^－3 s，远小于IMU的计算周期，故MCKF可以保证系统的实时性。

4 结语

针对复杂环境下GNSS/SINS组合导航系统中系统噪声及测量噪声为非高斯特性的问题，本文提出一种最大熵卡尔曼滤波(MCKF)算法。该算法采用最大熵准则以降低滤波器对异常信息的敏感性，进而提高系统的鲁棒性。MCKF算法中的状态和协方差矩阵的先验估计传播过程与KF相同，但采用迭代算法来更新后验估计。

仿真实验结果表明，当系统噪声及测量噪声满足混合高斯分布时，KF的性能高于MCKF；当测量值受到重尾脉冲噪声干扰时，MCKF的性能明显高于KF；当核带宽趋于无穷大时，MCKF的性能与KF等价。

为了在混合高斯噪声环境下使MCKF的性能与KF的性能相同，而在重尾脉冲噪声环境下使MCKF的性能优于KF，下一步将开展具有自适应特性核带宽的MCKF算法研究。