[发明专利]一种基于鲁棒无迹四元数卡尔曼滤波的极区粗对准方法

说明书

本发明公开了一种基于鲁棒无迹四元数卡尔曼滤波的极区粗对准方法、装置、系统以及计算机可读存储介质，包括：采集惯性传感器数据和GPS速度数据；根据横向坐标系的比力方程和GPS速度数据构建横向坐标系参考矢量和观测矢量；根据惯性传感器的数据、GPS速度数据以及横向坐标系下的参考矢量和观测矢量构建无迹卡尔曼滤波模型；构建抑制GPS野值的模值匹配权值函数；基于无迹四元数卡尔曼滤波模型与抑制GPS野值的模值匹配权值函数构建横向系鲁棒无迹四元数卡尔曼估计器模型；利用横向系鲁棒无迹四元数卡尔曼估计器模型进行极区初始姿态估计，完成粗对准。本发明采用横向系鲁棒无迹四元数卡尔曼估计器模型，去除GPS测量值中包含的野值，提高极区粗对准的精度。

技术领域

本发明涉及惯性导航技术领域，特别是涉及一种基于鲁棒无迹四元数卡尔曼滤波的极区粗对准方法、装置、系统以及计算机可读存储介质。

背景技术

为了提高粗对准精度，在非极区，众多学者提出了改进算法，如：最优化的初始姿态确定(OBA)方法，该方法能够充分利用观测和参考矢量，通过连续定姿的方式加快了粗对准的速度；接着武元新等人提出了速度/位置积分公式，使用外部辅助设备GPS构造观测矢量和参考矢量，通过OBA算法完成动基座初始对准；接着，基于Huber的M估计的鲁棒四元数卡尔曼滤波方法等被提出。但由于极区地理、气候和电磁条件的复杂性，这些传统的粗对准方法在极区不再适用。在极区，如格网系下姿态矩阵用惯性系分解的方法完成摇摆基座下粗对准；地球坐标系下GNSS辅助的OBA粗对准方法等，但以上极区粗对准方法不适用于低精度传感器，并且没有考虑复杂的极地环境对GPS性能的影响。

综上所述可以看出，如何抑制GPS野值来提高粗对准的精度是目前有待解决的问题。

发明内容

本发明的目的是提供一种基于鲁棒无迹四元数卡尔曼滤波的极区粗对准方法、装置、系统以及计算机可读存储介质，解决现有技术中无法抑制GPS野值对极区粗对准的影响，导致定位不准确的缺陷。

为解决上述技术问题，本发明提供一种基于鲁棒无迹四元数卡尔曼滤波的极区粗对准方法包括：获取惯性传感器数据和GPS速度数据；

根据横向坐标系的比力方程和所述GPS速度数据构建横向坐标系下的参考矢量和观测矢量；

根据所述惯性传感器的数据、所述GPS速度数据以及所述横向坐标系下的参考矢量和观测矢量构建无迹卡尔曼滤波模型；

基于实际速度测量值与理想值得关系构建抑制GPS野值的模值匹配权值函数；

基于所述无迹四元数卡尔曼滤波模型与所述抑制GPS野值的模值匹配权值函数构建横向系鲁棒无迹四元数卡尔曼估计器模型；

利用所述横向系鲁棒无迹四元数卡尔曼估计器模型进行极区初始姿态估计，完成粗对准过程。

优选地，所述采集惯性传感器数据和GPS速度数据包括：

采集所述惯性传感器中的陀螺输出值和加速度计输出值，采集GPS输出速度值；

将所述陀螺输出值、所述加速度计输出值和所述GPS输出速度值由传统坐标系转化为横向坐标系。

优选地，所述根据横向坐标系的比力方程和所述GPS速度数据构建参考矢量和观测矢量包括：

根据采集的所述惯性传感器数据、所述GPS速度数据以及横向坐标系的比力方程构建横向坐标系下参考矢量和观测矢量；

所述参考矢量为：

所述观测矢量为：