融合高斯过程回归的UKF估计方法

叶文，蔡晨光，杨平，李建利

（1.中国计量科学研究院 力学与声学计量科学研究所，北京100029；2.北京航空航天大学 仪器科学与光电工程学院，北京100083）

高精度位置姿态测量系统（Position and Orientation System，POS）本质是一个 SINS／GNSS非线性组合系统，其通过非线性滤波方法，如扩展卡尔曼滤波（Extended Kalman Filter，EKF）、无迹卡尔曼滤波（Unscented Kalman Filter，UKF），融合来自GNSS和SINS的位置、速度等运动参数，估计SINS的状态误差，进而为载荷提供可靠、高频、高精度的运动信息[1-2]。无论是线性滤波还是非线性滤波，都需要事先精确建立惯导误差模型和确定噪声统计特性[3-4]。然而，这存在 2个严重问题：①精确建立误差模型几乎不可能，因为系统复杂时变；②已知噪声统计特性会随着系统的工作状态变化而变化。众所周知，由于后处理平滑利用了所有可观测的信息，理论上其具有最高的估计精度[5-6]。因此，一方面，后处理平滑结果可作为基准数据；另一方面，滤波估计方法仍有提升的空间。然而，怎样提升滤波估计精度是一个很有挑战的难题。

随着机器学习的发展，该问题得以解决。尤其是以高斯过程回归（Gaussian Process Regression，GPR）为主的无参数回归方法，具有不确定度输出、非线性映射、随机性预测等优点，很适合与贝叶斯滤波方法进行结合[7-11]。因此，针对UKF不可能把系统的所有误差源都考虑到，导致UKF的预测和估计能力仍有局限性的问题，本文提出了一种增强高斯过程的UKF（Enhanced GPUKF，EGP-UKF）方法。该方法的核心思想是利用GP学习滤波估计值与后处理平滑估计值之间的残差。

本文详细论述了如何使用非参数GP学习状态模型和量测模型与真实值（平滑）之间的残差，以及如何无缝地把这些残差融合到UKF中。由此产生的EGP-UKF方法具有非参数GP的优点：①EGP-UKF方法取决于已建立的状态模型和量测模型，但不需要完全精确的数学模型，而且也取决于GP模型，EGP-UKF模型及其参数可以从较少的训练数据中学习得到；②EGP-UKF方法考虑了动态模型的不确定性。此外，UKF可避免因没有足够的训练数据引起的EGP-UKF更多不确定性。通过车载实验验证了EGP-UKF方法的估计性能，实验结果表明，该方法可明显提高 POS的滤波估计精度。

1 SINS／GNSS组合导航

1.1 非线性惯导误差模型及状态方程

本文采用基于角误差模型的非线性姿态误差方程，该非线性误差模型是UKF的基础。首先定义坐标系，i为惯性坐标系，p为平台坐标系，n为导航坐标系，b为载体坐标系，e为地球坐标系。

1.1.1 姿态误差微分方程

式中：φ=[φEφNφU]T描述数学平台坐标系与导航坐标系之间的失准角；Cpn为平台坐标系p到导航坐标系n的方向余弦矩阵；ωnin为投影在导航坐标系n下的导航坐标系n相对于惯性坐标系i的角速度；δωnin为其误差；εb=[εxεyεz]T为陀螺仪随机漂移在载体坐标系b的投影；为从载体坐标系b到导航坐标系n方向余弦阵Cnb的估计；（I-Cpn）ωnin为姿态误差微分方程中的非线性部分。

1.1.2 速度误差微分方程

式中：V=[VEVNVU]T为导航坐标系 n投影下的速度；δV为其误差为加速度计所测比力在载体坐标系b上的投影；为加速度计随机偏置误差在载体坐标系b下的投影为速度误差微分方程中的非线性项。

1.1.3 位置误差微分方程

式中：δL、δλ和 δH分别为纬度、经度和高度误差；RN和 RM分别为椭球卯酉圈和子午圈曲率半径。

1.1.4 惯性器件方程

惯性测量单元由3个陀螺仪和3个加速度计组Δ成。假设陀螺仪随机Δ误差ε和加速度计随机Δ误差 包括随机常数 εc、c和高斯白噪声 εg、g。虽然随机常值每次启动不同，但启动后是确定的。因此，陀螺仪和加速度计的随机误差如下：

SINS／GNSS组合导航系统的状态方程可以通过误差模型（包括姿态误差、速度误差、位置误差、惯性器件陀螺仪和加速度计的误差）获得，其对应的状态方程为式（1）～式（3）和式（5）。

1.2 量测方程

量测方程是一个离散的线性方程，通过SINS捷联解算的速度／位置和 GNSS的速度／位置差得到。

式中：xk为 POS的状态向量；VGE、VGN和 VGU分别为 GNSS的东向、北向和天向速度；LGNSS、λGNSS和HGNSS分别为GNSS测量所得的纬度、经度和高度；ZV（k）和HV分别为速度量测信息和速度量测矩阵；ZP（k）和HP分别为位置量测信息和位置量测矩阵；MV和NP分别为GNSS的速度和位置噪声。

1.3 UKF方法

UKF是Sigma点卡尔曼滤波的一种，通过UT非线性变换逼近非线性系统状态的后验均值和协方差，估计精度至少能达到二阶泰勒以上，且不需要计算雅可比矩阵。

2 高斯过程

GP是一组随机变量的集合，且集合中任意数量的随机变量都应满足联合高斯分布的随机过程，由均值函数和协方差函数确定[12-13]。

式中：xi，xj∈Rd为任意随机变量。

式中：xi为输入量，yi为输出量，它们分别组成了Xin和为均值为零的高斯白噪声。

训练集中的Yout和测试点x*的预测值y*的联合分布为

式中：K（Xin，Xin）为对称的协方差矩阵；σn为 n维高斯噪声矩阵；In为n维单位矩阵；μ*为均值；Σ*为方差。

为了实现最优的训练效果，通常采用高斯核函数，如下：

式中：θ=｛S，σ2f，σ2n｝为超参数，可通过对数极大似然函数求解后验分布获得；σ2f为信号方差。一般采用共轭梯度方法求解最优超参数。函数的偏导数如下：

式中：C=K（Xin，Xin）+σ2nIn；α=（K（Xin，Xin）+σ2nIn）-1Yout。

3 EGP-UKF模型

3.1 学习状态方程和量测方程

EGP-UKF是一种融合数学模型和数据模型各自优势的高精度滤波模型。利用GP学习UKF的状态方程和量测方程以及协方差阵Q和R。针对多维误差模型，采用 GP学习UKF中的每一个状态估计值和真值之间的差值，且每个GP的训练数据都是一组输入输出关系。采用GP学习状态模型的残差时，首先求解UKF滤波估计值和后处理平滑值；然后将状态模型中当前时刻的状态映射到状态变化量。量测模型由状态映射到观测，它们分别是由SINS／GNSS组合导航系统后处理平滑和差分GNSS计算得到的。状态模型和量测模型的训练数据集的形式分别表示为

式中：xsk+1为后处理平滑估计状态量；zsk+1为捷联

惯导解算结果和差分GNSS之差。采用GP学习状态模型和量测模型残差的数学模型如下：

3.2 EGP-UKF方法流程

经过上述分析，EGP-UKF方法由2个阶段组成：学习和估计。在学习残差模型阶段中，采用GP分别进行学习状态模型和量测模型的训练数据），可分别得到状态模型和量测模型的残差回归模型及其协方差阵；在估计残差模型阶段中，在UKF算法的基础上，采用GP分别对状态方程和量测方程的残差进行预测，并自适应获取其协方差阵。具体算法如下。

1）初始化

2）计算 Sigma点

3）时间更新

4）量测更新

5）状态更新

本文提出的EGP-UKF高精度滤波估计方法不仅不需要训练数据包含整个误差状态空间，也不需要精确的数学模型。正如算法中标注“*”的公式所示。EGP-UKF建模可以用来学习状态模型和量测模型，其优于参数模型，特别是当参数模型是不准确的。GP的残差模型由GPf和GPg组成，一旦模型被训练好，预测模型便可得出：

4 车载实验

为了评价本文EGP-UKF方法的性能，通过车载实验数据验证了UKF、GP-UKF和EGP-UKF的性能。车载所用的 SINS／GNSS组合导航系统是北京航空航天大学设计的 POS，其由 IMU、GNSS和PCS组成，如图1所示。此外，为了保证后处理平滑的精度，在实验过程中架设了GNSS地面基准站，如图1（c）所示。POS安装在实验车上，如图 1（b）所示。

为了保证后处理的精度且更好地评价EGPUKF的性能，在北京市昌平区的航空博物馆前进行了车载实验。原因如下：①面积开阔，无GNSS信号遮挡；②道路相对平直，约3 km长。在实验过程中，SINS／GNSS组合导航系统运行良好，采集所有相关数据，如IMU数据、GNSS数据和实时组合导航数据，时长约为2200 s。因此，IMU数据和差分GNSS数据的平滑结果可以作为真值，其中前 800 s用来训练 GP-UKF[14-15]和 EGP-UKF，最后1 400 s评价本文方法的性能。车载实验轨迹如图 1（d）所示。

图1 车载实验Fig.1 Vehicle-mounted experiment

图2～图 4给出了 EGP-UKF、UKF、GP-UKF在纬度误差、经度误差和高度误差上的对比结果。在图2～图4中，本文提出的EGP-UKF方法和另外2种方法在开始阶段的性能几乎相同，一方面，因为在静止部分参数模型与后处理平滑模型基本一致，另一方面在静止部分本文方法的噪声几乎和参数模型一致。如果发生了机动，EGP-UKF精度明显高于 UKF和 GP-UKF。因为 EGP-UKF不仅能自适应调节系统噪声和量测噪声，而且还考虑了模型不确定性，然而UKF未考虑模型不确定性和噪声的影响。尽管GP-UKF考虑了噪声的影响，但该方法严重依赖数据模型，未考虑系统本身的数学模型。机动之后，本文方法也明显优于其他2种方法，因为EGP-UKF在系统误差模型的基础上进一步利用了数据模型，抑制了系统误差模型不确定度的影响。事实上，模型的不确定主要由参数模型和相应的平滑之间的残差组成。在某种程度上，EGP-UKF可以学习和预测这种模型不确定性带来的残差，并减轻其带来的不利影响。

图2 纬度误差对比Fig.2 Comparison of latitude error

图3 经度误差对比Fig.3 Comparison of longitude error

图4 高度误差对比Fig.4 Comparison of altitude error

图5～图7表明，在东向速度误差和北向速度误差方面，EGP-UKF比UKF和GP-UKF的精度更高、更稳定。一方面，东向速度误差在图3所示经度误差一致；另一方面，北向速度误差变化和图2所示的纬度误差变化一致，符合惯性导航系统的误差传播定律。由图4和图7可知，高度误差和天向速度误差几乎相同。

图5 东向速度误差对比Fig.5 Comparison of east velocity error

图6 北向速度误差对比Fig.6 Comparison of north velocity error

图7 天向速度误差对比Fig.7 Comparison of up velocity error

图8～图10表明，在航向角误差、俯仰角误差和横滚角误差方面，本文 EGP-UKF方法明显优于UKF和GP-UKF。可以看出，EGP-UKF的收敛速度比UKF和GP-UKF更快。此外，在航向角误差方面 EGP-UKF与 UKF、GP-UKF几乎是相同的。

图8 航向角误差对比Fig.8 Comparison of heading angle error

图9 俯仰角误差对比Fig.9 Comparison of pitch angle error

图10 横滚角误差对比Fig.10 Comparison of roll angle error

本文定量分析了3种方法的性能。相比GPUKF和 UKF，本文 EGP-UKF在纬度误差和经度误差分别降低了 18.5%、24.6%和 9.1%、10.6%。在高度误差方面，这3种方法几乎相同。东向速度误差和北向速度误差分别降低了42.5%、43.3%和22.6%、24.3%。因为天向速度和高度的相关性，天向速度误差几乎是相同的。此外，航向角误差、俯仰角误差和横滚角误差分别降低了10.3%、30.1%、24.6%和3.4%、24.7%、28.2%。总之，EGP-UKF学习了UKF和其后处理平滑之间的残差模型，通过该残差模型提高了POS的测量精度。

在一般情况下，UKF的计算复杂度为状态向量的维数的二次方。然而，GP在学习和预测过程中的计算复杂度分别是训练数据的三次方和二次方，故 GP-UKF比 UKF运算速度慢，另外由于EGP-UKF融合了 GP和 UKF共同的优点，所以EGP-UKF的运算速度比GP-UKF慢。

5 结 论

1）针对传统非线性滤波模型不确定性的问题，提出了增强 GP的UKF方法。该方法既充分利用了GPR和UKF的优点，又克服了它们自身的缺点。

2）该方法的核心是通过非参数 GP学习UKF的状态方程、量测方程和后处理平滑之间的残差。

3）车载实验结果表明，与 UKF、GP-UKF相比，EGP-UKF具有更高的估计精度，因为 EGPUKF不仅具有更强的不确定度预测能力，而且具有更强的泛化能力。

4）如何提高EGP-UKF的实时性是未来的一个研究方向。