CN108759870B - 一种基于新型鲁棒广义高阶容积卡尔曼滤波的传递对准方法 - Google Patents

Abstract

本发明公开了一种基于新型鲁棒广义高阶容积卡尔曼滤波的传递对准方法。首先，考虑到系统的非线性，采用速度加姿态加角速度匹配方式，建立传递对准非线性数学模型。其次，主、子惯导分别进行惯导解算，主惯导的速度和姿态信息传输到子惯导的导航计算机，利用主、子惯导系统之间的速度误差、姿态误差和角速度误差构造观测量。然后，基于广义高阶容积卡尔曼滤波时间更新获得一步状态预测值和协方差，利用新息卡方检测方法对观测量受污染的程度进行判断，若判断结果超过了预设门限，则舍弃该部分观测量，跳过观测量重构过程和滤波更新过程；若判断结果未超过预设门限，则利用Huber方法对受污染观测量进行重构。最后，利用重构后的观测量进行量测更新，估计当前时刻的状态值和协方差，从而实现传递对准。本发明解决了观测量中存在混合高斯噪声和野值情况下的舰船快速高精度对准问题。

Description

一种基于新型鲁棒广义高阶容积卡尔曼滤波的传递对准方法

技术领域

本发明涉及捷联惯导系统初始对准领域，特别是涉及一种基于新型鲁棒广义高阶容积卡尔曼滤波的传递对准方法。

背景技术

惯性导航是一种推算定位方式，初始参数的准确性对惯性导航的精度具有重要影响，因此初始对准至关重要。传递对准由于具有对准精度高、速度快的优点成为目前较为常用的一种初始对准方式。传递对准的本质是建立误差模型并利用滤波算法对误差参数进行估计，由于实际系统一般具有非线性，因此非线性滤波方法成为研究热点之一。

非线性滤波的核心任务是对状态的后验概率密度函数进行计算，基于“对概率分布进行近似要比对非线性函数进行近似要容易”的认识,发展出包括无迹卡尔曼滤波(Unscented Kalman Filter，UKF)、高斯-埃尔米特卡尔曼滤波(Gauss-Hermite KalmanFilter，GHKF)、容积卡尔曼滤波(Cubature Kalman Filter，CKF)、高阶容积卡尔曼滤波(HCKF)、广义高阶容积卡尔曼滤波(GHCKF)等在内的多种次优非线性滤波方法。其中，GHCKF在获得更高滤波精度的同时，进一步克服了HCKF结构复杂、高阶扩展性差的问题。

GHCKF是基于l₂范数的最优估计，其假设前提是噪声为高斯白噪声,统计学表明其不具有鲁棒性。即当假设条件和现实参数不相符时，估计量都会发生明显的变化，而当实际参数和假设具有较大差别时甚至会造成滤波发散。在舰船传递对准的实际应用中，由于受到外界复杂环境的影响，观测量中往往存在非高斯强噪声和野值，此时GHCKF可能会失效。

Huber方法是一种可以用于解决在高斯分布附近存在一定对称干扰的随机量(即混合高斯分布)问题的实用方法。Huber方法结合最小二乘法和绝对和最小法来构造代价函数，即设置一个调节因子γ，在残差小于γ处应用最小二乘法，在残差大于γ处应用绝对和最小法。而新息卡方检测法可以利用新息对观测量中的突变干扰、野值等进行检测和隔离，通过一定门限将野值等受污染的观测检测出并剔除，以起到降低干扰的作用。

因此，为解决传递对准实际应用时，观测量中存在非高斯强噪声和野值的问题，本发明提出了一种基于新型鲁棒广义高阶容积卡尔曼滤波的传递对准方法。本发明将Huber方法和新息卡方检测法的思想用于传递对准中，首先进行标准的GHCKF时间更新，然后利用新息卡方检测方法预判断观测量受污染的情况，之后利用Huber方法对新息进行加权修正，并进行标准的GHCKF量测更新，从而实现了算法的鲁棒化。本发明可以解决观测噪声为非高斯噪声和观测量中存在野值的问题，从而提高舰船传递对准的精度。

发明内容

本发明的目的在于提供一种可应用于观测量中存在非高斯强噪声和野值情况下的舰船快速高精度传递对准方法。

实现本发明目的的技术方案为：一种基于新型鲁棒广义高阶容积卡尔曼滤波的传递对准方法，包括以下步骤：

步骤一：安装高精度主惯导系统和精度较低的子惯导系统，完成启动、预热准备；

步骤二：主、子惯导系统分别进行惯导解算，主惯导输出的速度、姿态和角速度信息传输到子惯导系统的导航计算机；

步骤三：建立大失准角情况下的舰船传递对准非线性模型；

步骤四：进行广义高阶容积卡尔曼滤波GHCKF时间更新，并利用新息卡方检测方法预判断观测量受污染的情况，从而剔除观测量中野值的影响。

步骤五：利用Huber方法对新息进行加权修正，之后进行GHCKF量测更新，从而实现算法的鲁棒性，完成传递对准。

在步骤四中，利用新息卡方检测方法预判断观测量受污染的情况，具体方法为：

基于GHCKF时间更新获得k时刻的状态预测值

和状态预测协方差P_k,k-1；

将k时刻的状态预测值

在非线性量测方程中进行传递，得到k时刻的量测预测值

和量测预测协方差P_zz,k/k-1；

计算新息v_k和观测有效性检测函数λ_k：

预设一个门限M，若λ＞M，说明该观测量中含有的有害信息超限应予以剔除，此时只进行时间更新，不进行量测更新；若λ＜M，则保留观测量，继续进行之后的步骤。

在步骤五中，利用Huber方法对新息进行加权修正，之后进行GHCKF量测更新，具体方法为：

计算变换新息：

其中，m为容积点数。

计算Huber权值函数：

其中，Huber调节因子γ设置为1.345，令Ψ＝diag[ψ(η_k,i)]。

计算l(k)，即：

l(k)＝(1-ε)(1-2Φ(-k))

其中，0≤ε≤1，Φ是标准高斯分布函数。

估计k时刻的状态值和协方差矩阵：

与现有技术相比，本发明的有益效果是：

本发明在舰船存在大失准角的情况下，将系统建模为非线性模型，并且针对观测量中存在非高斯强噪声和野值的问题，设计了一种基于新型鲁棒广义高阶容积卡尔曼滤波的传递对准方法，在标准GHCKF的基础上，利用新息卡方检测法对观测量受污染情况进行预判，利用Huber方法构造权值函数对新息进行加权修正，以提高GHCKF算法的鲁棒性，从而提高了传递对准的精度。

附图说明

图1为本发明流程示意图；

图2为Matlab仿真得到的安装误差角估计误差曲线；

图3为Matlab仿真得到的航向安装误差角估计误差曲线；

图4为蒙特卡罗仿真结果。

具体实施方式

下面结合附图1本发明流程示意图对本发明进一步说明。

为了验证本发明的有效性，利用Matlab对本发明的方法进行仿真。

首先，采用速度加姿态匹配方式，建立传递对准非线性模型，具体如下：

选取速度误差、量测失准角、安装误差角、加速度计漂移和陀螺漂移作为状态变量：

建立系统状态方程：

其中，n为导航坐标系；m系为主惯导载体坐标系；s系为子惯导载体坐标系；

为子惯导计算载体坐标系；δVⁿ为速度误差在导航坐标系的投影；

为主惯导载体坐标系到子惯导载体坐标系的方向余弦矩阵；

为主惯导载体坐标系到子惯导计算载体坐标系的方向余弦矩阵；

为主惯导载体坐标系到导航坐标系的方向余弦矩阵；

为子惯导测量的比力在其载体坐标系的投影；

为地球自转角速度在导航坐标系的投影；

为n系相对于地球坐标系的角速度在n系的投影；

为s系和m系之间的安装误差角；

为

系和m系之间的量测失准角；

为主惯导相对于导航坐标系的角速度在m系的投影；▽^s为加速度计常值漂移；w_v为加速度计随机漂移；ε^s为陀螺常值漂移；

为陀螺随机漂移。

选取主、子惯导间的速度误差δVⁿ、量测失准角

以及角速度误差

作为观测量：

量测方程为：

Z＝h(X)+V

其中，V为系统的观测噪声。

然后，设计新型鲁棒广义高阶容积卡尔曼滤波算法，具体如下：

对于如下一个离散非线性系统：

其中，x_k为系统状态矢量；z_k为观测矢量；w_k为系统噪声矢量，v_k为量测噪声矢量，均为零均值的高斯白噪声，且互不相关，即满足：

其中，Q_k为系统噪声序列的方差阵；R_k为量测噪声序列的方差阵；δ_kj为克罗内克函数。

新型鲁棒广义高阶容积卡尔曼滤波算法的具体实现步骤如下：

(1)基于GHCKF时间更新

GHCKF采用广义五阶容积准则，容积点数为m＝2n²+1

假设k-1时刻的状态x_k-1的统计特性已知，先对P_k-1做Cholesky分解：

计算容积点：

计算容积点权重：

估计k时刻的状态预测值：

估计k时刻的状态预测协方差阵：

(2)将

在非线性方程中传递

对P_k/k-1做Cholesky分解：

计算容积点：

计算经系统量测方程传递后的容积点：

Z_i,k/k-1＝h(X_i,k/k-1)i＝1,2…,2n²+1

估计k时刻的量测预测值：

估计k时刻的量测预测协方差阵：

估计k时刻的一步预测互相关协方差阵：

(3)新息卡方检测预判断

计算新息：

计算有效性检测函数：

预先设置一个门限M，若λ＞M，说明该观测量中含有的有害信息超限应予以剔除，此时只进行时间更新，不进行量测更新；若λ＜M，则保留观测量，继续进行之后的步骤。

(4)计算变换新息

(5)计算Huber权值函数

其中，Huber调节因子γ设置为1.345，令Ψ＝diag[ψ(η_k,i)]。

(6)计算l(k)

l(k)＝(1-ε)(1-2Φ(-k))

其中，0≤ε≤1，Φ是标准高斯分布函数。

(7)估计k时刻的状态值和协方差矩阵

最后，仿真验证本发明的有效性，舰船三轴摇摆模型：

式中，ψ，θ，γ分别表示航向角、纵摇角和横摇角；ψm，θ_m，γm为摇摆角幅值；ω_y，ω_p，ω_r为摇摆角频率；T_i＝2π/ωi,(i＝y,p,r)为摇摆周期；

为初始姿态角；K为初始航向。

仿真参数设置如下：

摇摆角幅值：ψ_m＝5°，θ_m＝15°，γ_m＝10°；

摇摆周期：T_y＝8s，T_p＝12s，T_r＝6s；

初始姿态角：

初始航向：K＝30°；

初始纬度

初始经度λ＝126.6705°；

误差角为：

陀螺常值漂移为ε_x＝ε_y＝ε_z＝0.01°/h，随机漂移为0.001°/h；

加速度计随机常值偏置为10^-4g，加速度计随机漂移为10^-5g；

舰船以10n mile/h的速度匀速直航；

滤波周期：0.05s；

观测噪声服从混合高斯分布：

观测量中随机加入野值。

滤波器初始条件，包括状态估计协方差阵P₀、系统噪声方差阵Q₀及量测噪声方差阵R₀，设定如下：

P₀＝diag{(0.1m/s)²,(0.1m/s)²,(1°)²,(1°)²,(10°)²,(1°)²,(1°)²,(10°)²,

(1×10^-4g₀)²,(1×10^-4g₀)²,(0.01°/h)²,(0.01°/h)²,(0.01°/h)²}

Q₀＝diag{(1×10^-5g₀)²,(1×10^-5g₀)²,(0.001°/h)²,(0.001°/h)²,(0.001°/h)²}

R₀＝diag{(0.02m/s)²,(0.02m/s)²,(0.0004°)²,(0.0004°)²,(0.0004°)²}

仿真结果：

以上述仿真条件，利用CKF、HCKF和本发明方法(RGHCKF)三种滤波算法估计安装误差角，仿真结果如图2、图3和图4所示。

由图2和图3可以看出，在观测量中存在混合高斯噪声和野值的情况下，CKF滤波估计误差曲线波动很大，收敛速度很慢，滤波精度降低，HCKF可以降低混合高斯噪声和野值的影响，误差估计曲线可以较快收敛，并且估计精度有所提高，而采用本发明方法，误差估计曲线可以更快收敛，估计精度也比HCKF更高。由图4蒙特卡罗仿真结果可以看出，采用传统CKF滤波估计安装误差角，估计误差在5角分左右，采用HCKF滤波，估计误差可达到2角分以下，而采用本文方法，估计误差可达到0.5角分以下。综上所述，本发明提供的方法，可以在观测量中存在混合高斯噪声和野值的情况下，实现快速高精度对准。

Claims (1)

1.一种基于新型鲁棒广义高阶容积卡尔曼滤波的传递对准方法，其特征在于，包括以下步骤：

步骤一：安装高精度主惯导系统和精度较低的子惯导系统，完成启动、预热准备；

步骤二：主、子惯导系统分别进行惯导解算，主惯导输出的速度、姿态信息传输到子惯导系统的导航计算机；

步骤三：建立大失准角情况下的舰船传递对准非线性模型；

步骤四：进行广义高阶容积卡尔曼滤波GHCKF时间更新，并利用新息卡方检测方法预判断观测量受污染的情况，从而剔除观测量中野值的影响；

基于GHCKF时间更新获得k时刻的状态预测值

和状态预测协方差P_k,k-1；

将k时刻的状态预测值

在非线性量测方程中进行传递，得到k时刻的量测预测值

和量测预测协方差P_zz,k/k-1；

计算新息v_k和观测有效性检测函数λ_k：

预设一个门限M，若λ_k＞M，说明该观测量中含有的有害信息超限应予以剔除，此时只进行时间更新，不进行量测更新；若λ_k＜M，则保留观测量，继续进行之后的步骤；

步骤五：利用Huber方法对新息进行加权修正，之后进行GHCKF量测更新，从而实现算法的鲁棒性，完成传递对准；

在步骤五中，计算变换新息：

η_k＝(P_zz,k/k-1·m)^-1/2

其中，m为容积点数；

计算Huber权值函数：

其中，Huber调节因子γ设置为1.345，令Ψ＝diag[ψ(η_k,i)]；

计算l(k)，即：

l(k)＝(1-ε)(1-2Φ(-k))

其中，0≤ε≤1，Φ是标准高斯分布函数；

估计k时刻的状态值和协方差矩阵：

Images