CN108225373A - 一种基于改进的5阶容积卡尔曼的大失准角对准方法 - Google Patents

Abstract

本发明公开了一种基于改进的5阶容积卡尔曼的大失准角对准方法，该方法根据k时刻的量测值z_k计算k时刻的递推量测协方差阵A_k和量测协方差矩阵B_k，进而得到新息反馈系数α_k，用α_k更新下一滤波周期的一步预测状态协方差矩阵P′_k|k‑1，从而计算k时刻的估计值；重复上述步骤，得到每一时刻的状态估计。该方法在提高新息反馈效率的前提下引入反馈监督机制，进而提高对准的精度和稳定性。

Description

一种基于改进的5阶容积卡尔曼的大失准角对准方法

技术领域

本发明属于导航技术领域，具体涉及一种失准角的对准方法。

背景技术

初始对准是导航的关键技术和导航解算的前提，对准的精度高低一定程度上决定了导航的精度。初始对准一般分为粗对准和精对准，所谓粗对准是通过解析法、非线性滤波等方法把大失准角收敛到小角度，然后再用罗经对准、非线性滤波等方法把安装误差、器件误差和建模等误差综合计算后得到更精准的初始姿态角。

非线性滤波作为一种有效的状态估计算法在初始对准领域得到了广泛的应用。容积卡尔曼滤波(Cubature Kalman Filter，CKF)克服了传统的扩展卡尔曼滤波器(ExtendedKalman filter，EKF)和无迹卡尔曼(Unscented Kalman Filter，UKF)的截断误差和超参数等缺点，具有更好的数值稳定性和滤波精度。传统CKF可以精确估计三阶非线性函数的前两阶矩，更高阶数的则有截断误差。为了提高算法在强非线性场合下的精度，5阶CKF得到了发展。在实际工程应用中，滤波模型、***噪声和量测噪声往往都不能得到准确地建模，此时即便是5阶CKF也存在滤波精度低下或者滤波发散的问题。为了提高算法的鲁棒性，需要在传统CKF的基础上进行改进。当量测方程为非线性时，可通过迭代法提高新息的反馈效率；当量测方程为线性时，迭代法失效。初始对准的量测往往是以速率误差、位置误差为观测量的，量测方程此时为线性。为了提高非线性滤波在初始对准中的性能，可以通过引入创新算法，比如改进新息、新息误差和增益，进而改善新息对于状态估计的作用。但新息的反馈的强度需要跟踪、监督，当反馈噪声较大时可能导致滤波的错误收敛或发散。目前尚未有有效的监督机制来控制新息的反馈。

发明内容

发明目的：针对现有技术中存在的问题，本发明提供了一种基于改进的5阶容积卡尔曼的大失准角对准方法，该方法在提高新息反馈效率的前提下引入反馈监督机制，进而提高对准的精度和稳定性。

技术方案：本发明采用如下技术方案：

一种基于改进的5阶容积卡尔曼的大失准角对准方法，包括如下步骤：

(1)根据k时刻的量测值z_k计算k时刻的递推量测协方差矩阵A_k，步骤为：

计算k时刻的新息γ_k：γ_k＝z_k-HP_k|k-1；

计算k时刻的新息协方差矩阵Z_k：

计算k时刻的递推量测协方差矩阵A_k：A_k＝Z_k-R；

其中H为对准量测方程，P_k|k-1为当前滤波周期的一步预测协方差矩阵，b为渐消因子，R为量测噪声矩阵；T为向量或矩阵的转置算子；

(2)计算k时刻的量测协方差矩阵B_k和新息反馈系数α_k，更新下一滤波周期的一步预测状态协方差矩阵P′_k|k-1，计算式为：

B＝HP_k|k-1(H)^T

P′_k|k-1＝α_k·P_k|k-1

其中tr(·)为求矩阵的迹运算；

(3)计算k时刻的估计值；

(4)重复上述步骤，得到每一时刻的状态估计。

作为优选方式，在步骤(4)完成后还包括如下步骤：

计算每个滤波周期的方位失准角的梯度

找出方位失准角梯度的最大值所在的滤波周期值T_max：

在t＝1,...,T_max滤波周期内重新对IMU数据执行步骤1-4进行滤波；从t＝T_max+1,...,T_end滤波周期使用传统5阶CKF进行滤波，其中diff(·)为梯度算子，abs(·)为求绝对值算子，T_end表示最后一个滤波周期。

所述量测值为速度误差或位置误差。

渐消因子b的取值范围为[0.5,1)。

步骤(3)具体包括如下步骤：

其中K_k为滤波增益，为状态、量测互协方差阵，为量测协方差阵，P_k|k为状态后验协方差阵，为状态后验值，为量测估计值，z_k为k时刻的量测值。

有益效果：与现有技术相比，本发明公开的基于改进的5阶容积卡尔曼的大失准角对准方法结合创新的短时量测协方差误差系数反馈和迭代渐消因子方法对5阶CKF算法进行改进，提高了非线性滤波在模型误差未准确建模的环境下的对准精度和稳定性。

附图说明

图1为本发明实施例中大失准角对准方法的流程图。

具体实施方式

本发明公开了一种基于改进的5阶容积卡尔曼的大失准角对准方法，下面结合附图进一步阐述本发明。

如图1所示，一种基于改进的5阶容积卡尔曼的大失准角对准方法，包括如下步骤：

(1)根据k时刻的量测值z_k计算k时刻的递推量测协方差矩阵A_k；

假设H为对准量测方程，P_k|k-1为当前滤波周期的一步预测协方差矩阵，b为渐消因子，b的取值范围一般为[0.5,1)，R为量测噪声矩阵，量测值为速度误差或位置误差，计算如下：

计算k时刻的新息γ_k：γ_k＝z_k-HP_k|k-1；

计算k时刻的新息协方差矩阵Z_k：

计算k时刻的递推量测协方差矩阵A_k：A_k＝Z_k-R；

其中T为向量或矩阵的转置算子；

(2)计算k时刻的量测协方差矩阵B_k和新息反馈系数α_k，更新下一滤波周期的一步预测状态协方差矩阵P′_k|k-1，计算式为：

B＝HP_k|k-1(H)^T

P′_k|k-1＝α_k·P_k|k-1

其中tr(·)为求矩阵的迹运算；

(3)计算k时刻的估计值；

其中K_k为滤波增益，为状态、量测互协方差阵，为量测协方差阵，P_k|k为状态后验协方差阵，为状态后验值，为量测估计值，z_k为k时刻的量测值。

(4)重复上述步骤，得到每一时刻的状态估计，完成全时反馈的改进滤波。

上述方法是一种短时反馈滤波方法，可以在对准初期有效地利用量测新息，使方位大失准角能够尽快地收敛；待失准角收敛到小角时如果继续使用反馈滤波，可能由于过反馈而造成振荡或者发散，此时反馈滤波不再是必要的了。由此，作为一种改进方案，在步骤(4)完成后还包括如下步骤：

计算每个滤波周期的方位失准角的梯度

找出方位失准角梯度的最大值所在的滤波周期值T_max：

在t＝1,...,T_max滤波周期内重新对惯性测量装置(Inertial Measurement Unit，IMU)数据执行步骤1-4进行滤波；从t＝T_max+1,...,T_end滤波周期使用传统5阶CKF进行滤波，其中diff(·)为梯度算子，abs(·)为求绝对值算子，T_end表示最后一个滤波周期。

Claims (5)

1.一种基于改进的5阶容积卡尔曼的大失准角对准方法，其特征在于，包括如下步骤：

(1)根据k时刻的量测值z_k计算k时刻的递推量测协方差矩阵A_k，步骤为：

计算k时刻的新息γ_k：γ_k＝z_k-HP_k|k-1；

计算k时刻的新息协方差矩阵Z_k：

计算k时刻的递推量测协方差矩阵A_k：A_k＝Z_k-R；

其中H为对准量测方程，P_k|k-1为当前滤波周期的一步预测协方差矩阵，b为渐消因子，R为量测噪声矩阵；T为向量或矩阵的转置算子；

(2)计算k时刻的量测协方差矩阵B_k和新息反馈系数α_k，更新下一滤波周期的一步预测状态协方差矩阵P_k′_|k-1，计算式为：

B＝HP_k|k-1(H)^T

P_k′_|k-1＝α_k·P_k|k-1

其中tr(·)为求矩阵的迹运算；

(3)计算k时刻的估计值；

(4)重复上述步骤，得到每一时刻的状态估计。

2.根据权利要求1所述的基于改进的5阶容积卡尔曼的大失准角对准方法，其特征在于，在步骤(4)完成后还包括如下步骤：

计算每个滤波周期的方位失准角的梯度

找出方位失准角梯度的最大值所在的滤波周期值T_max：

在t＝1,...,T_max滤波周期内重新对IMU数据执行步骤1-4进行滤波；从t＝T_max+1,...,T_end滤波周期使用传统5阶CKF进行滤波，其中diff(·)为梯度算子，abs(·)为求绝对值算子，T_end表示最后一个滤波周期。

3.根据权利要求1所述的基于改进的5阶容积卡尔曼的大失准角对准方法，其特征在于，所述量测值为速度误差或位置误差。

4.根据权利要求1所述的基于改进的5阶容积卡尔曼的大失准角对准方法，其特征在于，渐消因子b的取值范围为[0.5,1)。

5.根据权利要求1所述的基于改进的5阶容积卡尔曼的大失准角对准方法，其特征在于，步骤(3)具体包括如下步骤：

其中K_k为滤波增益，为状态、量测互协方差阵，为量测协方差阵，P_k|k为状态后验协方差阵，为状态后验值，为量测估计值，z_k为k时刻的量测值。