[go: up one dir, main page]

CN114812614A - 一种基于鲁棒无迹四元数卡尔曼滤波的极区粗对准方法 - Google Patents

一种基于鲁棒无迹四元数卡尔曼滤波的极区粗对准方法 Download PDF

Info

Publication number
CN114812614A
CN114812614A CN202210466480.5A CN202210466480A CN114812614A CN 114812614 A CN114812614 A CN 114812614A CN 202210466480 A CN202210466480 A CN 202210466480A CN 114812614 A CN114812614 A CN 114812614A
Authority
CN
China
Prior art keywords
coordinate system
gps
quaternion
unscented
lateral
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Pending
Application number
CN202210466480.5A
Other languages
English (en)
Inventor
黄玲
徐祥
葛浩冉
赵鹤鸣
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Suzhou University
Original Assignee
Suzhou University
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Suzhou University filed Critical Suzhou University
Priority to CN202210466480.5A priority Critical patent/CN114812614A/zh
Publication of CN114812614A publication Critical patent/CN114812614A/zh
Pending legal-status Critical Current

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C25/00—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass
    • G01C25/005—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass initial alignment, calibration or starting-up of inertial devices

Landscapes

  • Engineering & Computer Science (AREA)
  • Manufacturing & Machinery (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Navigation (AREA)
  • Position Fixing By Use Of Radio Waves (AREA)

Abstract

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

Description

一种基于鲁棒无迹四元数卡尔曼滤波的极区粗对准方法
技术领域
本发明涉及惯性导航技术领域，特别是涉及一种基于鲁棒无迹四元数卡尔曼滤波的极区粗对准方法、装置、系统以及计算机可读存储介质。
背景技术
为了提高粗对准精度，在非极区，众多学者提出了改进算法，如：最优化的初始姿态确定(OBA)方法，该方法能够充分利用观测和参考矢量，通过连续定姿的方式加快了粗对准的速度；接着武元新等人提出了速度/位置积分公式，使用外部辅助设备GPS构造观测矢量和参考矢量，通过OBA算法完成动基座初始对准；接着，基于Huber的M估计的鲁棒四元数卡尔曼滤波方法等被提出。但由于极区地理、气候和电磁条件的复杂性，这些传统的粗对准方法在极区不再适用。在极区，如格网系下姿态矩阵用惯性系分解的方法完成摇摆基座下粗对准；地球坐标系下GNSS辅助的OBA粗对准方法等，但以上极区粗对准方法不适用于低精度传感器，并且没有考虑复杂的极地环境对GPS性能的影响。
综上所述可以看出，如何抑制GPS野值来提高粗对准的精度是目前有待解决的问题。
发明内容
本发明的目的是提供一种基于鲁棒无迹四元数卡尔曼滤波的极区粗对准方法、装置、系统以及计算机可读存储介质，解决现有技术中无法抑制GPS野值对极区粗对准的影响，导致定位不准确的缺陷。
为解决上述技术问题，本发明提供一种基于鲁棒无迹四元数卡尔曼滤波的极区粗对准方法包括：获取惯性传感器数据和GPS速度数据；
根据横向坐标系的比力方程和所述GPS速度数据构建横向坐标系下的参考矢量和观测矢量；
根据所述惯性传感器的数据、所述GPS速度数据以及所述横向坐标系下的参考矢量和观测矢量构建无迹卡尔曼滤波模型；
基于实际速度测量值与理想值得关系构建抑制GPS野值的模值匹配权值函数；
基于所述无迹四元数卡尔曼滤波模型与所述抑制GPS野值的模值匹配权值函数构建横向系鲁棒无迹四元数卡尔曼估计器模型；
利用所述横向系鲁棒无迹四元数卡尔曼估计器模型进行极区初始姿态估计，完成粗对准过程。
优选地，所述采集惯性传感器数据和GPS速度数据包括：
采集所述惯性传感器中的陀螺输出值和加速度计输出值，采集GPS输出速度值；
将所述陀螺输出值、所述加速度计输出值和所述GPS输出速度值由传统坐标系转化为横向坐标系。
优选地，所述根据横向坐标系的比力方程和所述GPS速度数据构建参考矢量和观测矢量包括：
根据采集的所述惯性传感器数据、所述GPS速度数据以及横向坐标系的比力方程构建横向坐标系下参考矢量和观测矢量；
所述参考矢量为：
Figure BDA0003624414630000021
所述观测矢量为：
Figure BDA0003624414630000022
其中，αM为横向坐标系下参考矢量，βM为横向坐标系下观测矢量，fb为采集的传感器加速度计数据，b(0)为初始对准起始时刻载体坐标系，并且相对惯性系不变，b(tk)为tk时刻的载体坐标系，
Figure BDA0003624414630000023
为b系姿态矩阵，nt(0)为初始对准起始时刻横向坐标系，并且相对惯性系不变，nt(tk)为tk时刻的横向坐标系，
Figure BDA0003624414630000024
为nt系姿态矩阵，nt为横向坐标系，
Figure BDA0003624414630000031
为nt系下得重力加速度矢量，
Figure BDA0003624414630000032
为nt系下et系相对于i系的角速度，et为横向地球坐标系，i为惯性坐标系，ωie为地球自转角速度，Lt为横向纬度，λt为横向经度，
Figure BDA0003624414630000033
为横向坐标系下北向速度，
Figure BDA0003624414630000034
为横向坐标系下东向速度，
Figure BDA0003624414630000035
为横向坐标系下速度，Δtg为GPS采样时间间隔，Δts为SINS的采样时间间隔，且有
Figure BDA0003624414630000036
Figure BDA0003624414630000037
G为GPS采样间隔和SINS采样间隔的关系系数，
Figure BDA0003624414630000038
为正实数集合，tM为GPS的采样时间，M为更新时间间隔的次数，tM＝t＝MΔtg，Δtg＝tk+1-tk，k＝0,1，2，3，...,M-1。
优选地，所述无迹卡尔曼滤波模型包括预测模型和量测模型；
所述无迹卡尔曼滤波的预测模型为：
Figure BDA0003624414630000039
其中，
Figure BDA00036244146300000310
为陀螺仪的输出，εb(t)为陀螺零偏，η为量测噪声，是均值为零的高斯白噪声，
Figure BDA00036244146300000318
为微分后的陀螺零偏，
Figure BDA00036244146300000311
为横向坐标系姿态方向余弦矩阵，
Figure BDA00036244146300000312
为横向坐标系相对于惯性系的角速度，
Figure BDA00036244146300000313
为横向坐标系相对于惯性系的角速度。
将所述无迹卡尔曼滤波的预测模型转换为四元数表示的预测模型，其表达式为：
Figure BDA00036244146300000314
其中，
Figure BDA00036244146300000315
为横向坐标系k-1时刻的姿态四元数，εk-1为k-1时刻的陀螺零偏，
Figure BDA00036244146300000316
为陀螺在b系中的测量角速度，
Figure BDA00036244146300000317
为横向坐标系相对于惯性系的角速度，b为载体坐标系，h为用四元数表示的离散时间姿态运动学方程。
所述无迹卡尔曼滤波的量测模型为：
Figure BDA0003624414630000041
Figure BDA0003624414630000042
其中，
Figure BDA0003624414630000043
为
Figure BDA0003624414630000044
的方向余弦矩阵，
Figure BDA0003624414630000045
为tM时刻的横向坐标系姿态四元数，tM为GPS的采样时间，M为更新时间间隔的次数，η为量测噪声，T为矩阵转置。
优选地，所述基于实际速度测量值与理想值得关系构建抑制GPS野值的模值匹配权值函数包括：
建立实际速度测量值与理想值的关系式：
Figure BDA0003624414630000046
Figure BDA0003624414630000047
将所述实际速度测量值与理想值的关系式代入所述量测模型得到
Figure BDA0003624414630000048
利用Huber鲁棒滤波理论中幅值匹配方法，计算得到所述抑制GPS野值的模值匹配权值函数：
Figure BDA0003624414630000049
其中，
Figure BDA00036244146300000410
为实际测量速度，
Figure BDA00036244146300000411
为真实速度，
Figure BDA00036244146300000412
为tM时刻高斯噪声与野值的混合噪声，
Figure BDA00036244146300000413
为实际观测矢量，βM为无误差观测矢量，γ为高斯阈值，ξM为归一化残差，其表达式为：
Figure BDA00036244146300000414
Figure BDA00036244146300000415
σ为随机变量方差，var[εM]为εM的方差，εM为归一化残差。
优选地，所述基于所述无迹四元数卡尔曼滤波模型与所述抑制GPS野值的模值匹配权值函数构建横向系鲁棒无迹四元数卡尔曼估计器模型包括：
根据所述基于所述无迹四元数卡尔曼滤波的预测模型和量测模型以及所述GPS野值的模值匹配权值函数构建横向系鲁棒无迹四元数卡尔曼估计器模型：
Figure BDA0003624414630000051
Figure BDA0003624414630000052
其中，
Figure BDA0003624414630000053
为改进后的观测矢量，
Figure BDA0003624414630000054
为改进前的实际观测矢量
Figure BDA0003624414630000055
为量测预测的均值。
优选地，所述利用所述横向系鲁棒无迹四元数卡尔曼估计器模型进行极区初始姿态估计，完成粗对准过程包括：
初始化算法，给定k-1时刻的状态的估计向量
Figure BDA0003624414630000056
状态协方差矩阵Pk-1，估计四元数
Figure BDA0003624414630000057
利用所述估计向量
Figure BDA0003624414630000058
和所述状态协方差矩阵Pk-1计算Sigma点集χk-1；
根据所述Sigma点集χk-1和所述状态向量计算得到四元数的Sigma点集
Figure BDA0003624414630000059
基于所述四元数预测模型，计算得到预测四元数Sigma点集和预测误差四元数Sigma点集，计算预测更新的状态向量与状态协方差矩阵；
将所述预测四元数Sigma点集输入所述量测模型中计算得到量测预测Sigma点集、量测预测的均值和协方差矩阵；
根据所述协方差矩阵计算滤波增益；
基于所述滤波增益和所述抑制GPS野值的模值匹配权值函数，计算观测向量和估计向量，并更新状态估计协方差阵。
本发明还提供了一种基于鲁棒无迹四元数卡尔曼滤波的极区粗对准的装置，包括：
获取模块，用于获取惯性传感器数据和GPS速度数据；
构建矢量模块，用于根据横向坐标系的比力方程和所述GPS速度数据构建横向坐标系下的参考矢量和观测矢量；
构建模型模块，用于根据惯性传感器的数据、所述GPS速度数据以及所述横向坐标系下的参考矢量和观测矢量构建无迹卡尔曼滤波模型；
构建权值函数模块，用于基于实际速度测量值与理想值得关系构建抑制GPS野值的模值匹配权值函数；
构建估计器模块，用于基于所述无迹四元数卡尔曼滤波模型与所述抑制GPS野值的模值匹配权值函数构建横向系鲁棒无迹四元数卡尔曼估计器模型；
姿态估计模块，用于利用所述横向系鲁棒无迹四元数卡尔曼估计器模型进行极区初始姿态估计，完成粗对准过程。
本发明还提供了一种极区定位系统，包括：
惯性传感器，安装于被定位载体上，用于采集载体的陀螺输出值和加速度计输出值；
GPS定位器，安装于所述极区定位系统中，用于定位所述载体的速度数据；
处理器，用于执行上述权利要求1-7任一项所述的基于鲁棒无迹四元数卡尔曼滤波的极区粗对准方法的步骤，完成极区粗对准，并将数据上传；
控制显示器：用于接收并显示所述处理器上传的极区粗对准的定位结果。
本发明还提供了一种计算机可读存储介质，所述计算机可读存储介质上存储有计算机程序，所述计算机程序被处理器执行时实现上述一种基于鲁棒无迹四元数卡尔曼滤波的极区粗对准方法的步骤。
本发明所提供的一种基于鲁棒无迹四元数卡尔曼滤波的极区粗对准方法和装置，首先采集惯性传感器数据和GPS速度数据，然后根据采集的数据构建参考矢量和观测矢量，采用横向系力学编排进行极区粗对准，解决了传统初始对准方法的极区失效问题，再根据参考矢量和观测矢量以及GPS速度数据构建无迹卡尔曼滤波模型，把陀螺零偏作为状态变量，消除了低精度传感器陀螺仪的累积误差，最后采用鲁棒无迹四元数估计器进行极区初始姿态估计，完成极区粗对准过程。采用横向系鲁棒无迹四元数卡尔曼估计器模型，去除了GPS测量值中包含的野值，提高极区粗对准的精度。本发明采用横向坐标系鲁棒无迹卡尔曼滤波的方法，能够消除观测矢量中的野值，同时陀螺零偏导致的航向角误差的波动特性也得到了改善，具有较好的鲁棒性和对陀螺零偏累积误差抑制的有效性，具有很好的极区适用性。
附图说明
为了更清楚的说明本发明实施例或现有技术的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单的介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。
图1为本发明所提供的一种基于鲁棒无迹四元数卡尔曼滤波的极区粗对准方法的第一种具体实施例的流程图；
图2为本发明所提供的一种基于鲁棒无迹四元数卡尔曼滤波的极区粗对准方法的第二种具体实施例的流程图；
图3为本发明载体的运动轨迹；
图4为本发明载体真实姿态角与GPS速度测量值；
图5为改进前后的矢量观测值的对比图；
图6为本发明方法得到的陀螺零偏估计曲线图；
图7为本发明方法得到的横向系俯仰角误差曲线图；
图8为本发明方法得到的横向系横滚角误差曲线图；
图9为本发明方法得到的横向系航向角误差曲线图；
图10为本发明实施例提供的一种基于鲁棒无迹四元数卡尔曼滤波的极区粗对准的装置的结构框图。
具体实施方式
本发明的核心是提供一种基于鲁棒无迹四元数卡尔曼滤波的极区粗对准方法和装置，采用鲁棒无迹四元数估计器去除GPS测量值中包含的野值，提高极区粗对准的精度。
为了使本技术领域的人员更好地理解本发明方案，下面结合附图和具体实施方式对本发明作进一步的详细说明。显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
请参考图1，图1为本发明所提供的一种基于鲁棒无迹四元数卡尔曼滤波的极区粗对准方法的第一种具体实施例的流程图；具体操作步骤如下：
步骤S101：采集惯性传感器数据和GPS速度数据；
步骤S102：根据横向坐标系的比力方程和所述GPS速度数据构建横向坐标系下的参考矢量和观测矢量；
步骤S103：根据惯性传感器的数据、所述GPS速度数据以及所述横向坐标系下的参考矢量和观测矢量构建无迹卡尔曼滤波模型；
步骤S104：基于实际速度测量值与理想值得关系构建抑制GPS野值的模值匹配权值函数；
步骤S105：基于所述无迹四元数卡尔曼滤波模型与所述抑制GPS野值的模值匹配权值函数构建横向系鲁棒无迹四元数卡尔曼估计器模型；
步骤S106：利用所述横向系鲁棒无迹四元数卡尔曼估计器模型进行极区初始姿态估计，完成粗对准过程。
本实施例所提供的一种基于鲁棒无迹四元数卡尔曼滤波的极区粗对准方法和装置，首先采集惯性传感器数据和GPS速度数据，然后根据采集的数据构建参考矢量和观测矢量，采用横向系力学编排进行极区粗对准，解决了传统初始对准方法的极区失效问题，再根据参考矢量和观测矢量以及GPS速度数据构建无迹卡尔曼滤波模型，把陀螺零偏作为状态变量，消除了低精度传感器陀螺仪零偏的累积误差，最后采用鲁棒无迹四元数估计器进行极区初始姿态估计，完成极区粗对准过程。采用横向系鲁棒无迹四元数卡尔曼估计器模型，去除了GPS测量值中包含的野值，提高极区粗对准的精度。本发明采用横向坐标系鲁棒无迹卡尔曼滤波的方法，能够消除观测矢量中的野值，同时陀螺零偏导致的航向角误差的波动特性也得到了改善，具有较好的鲁棒性和对陀螺零偏累积误差抑制的有效性，具有很好的极区适用性。
基于上述实施例，本实施例更加详细的说明了极区粗对准的方法，请参考图2，图2为本发明所提供的一种基于鲁棒无迹四元数卡尔曼滤波的极区粗对准方法的第二种具体实施例的流程图；具体操作步骤如下：
步骤S201：获取载体的惯性传感器和GPS输出速度值的数据；
根据图3中载体的运动轨迹，在载体运动过程中采集所述惯性传感器中的陀螺输出值和加速度计输出值，采集GPS输出速度值；
将所述陀螺输出值、所述加速度计输出值和所述GPS输出速度值由传统坐标系转化到横向坐标系，采集的姿态和速度数据如图4所示。
在进行初始粗对准前，需要将获得的传感器数据和GPS输出速度信息等由传统坐标系转换到横向坐标系中，接着进行后续步骤中横向观测矢量构建及相关横向系鲁棒无迹四元数卡尔曼滤波等的相关计算。
步骤S202：根据横向坐标系的比力方程和GPS速度量测构建横向系下参考矢量和观测矢量；
所述参考矢量为：
Figure BDA0003624414630000091
所述观测矢量为：
Figure BDA0003624414630000092
其中，αM为横向坐标系参考矢量，βM为横向坐标系观测矢量，M为GPS采样的当前时间，是一个整数，b(0)为初始对准起始时刻载体坐标系，并且相对惯性系不变，b(tk)为tk时刻的载体坐标系，fb为采集的传感器加速度计数据，
Figure BDA0003624414630000101
为b系姿态矩阵，nt(0)为初始对准起始时刻横向坐标系，并且相对惯性系不变，nt(tk)为tk时刻的横向坐标系，
Figure BDA0003624414630000102
为nt系姿态矩阵，nt为横向坐标系，
Figure BDA0003624414630000103
Figure BDA0003624414630000104
为nt系下得重力加速度矢量，
Figure BDA0003624414630000105
Figure BDA0003624414630000106
为nt系下et系相对于i系的角速度，et为横向地球坐标系，i为惯性坐标系，ωie为地球自转角速度，Lt为横向纬度，λt为横向经度，
Figure BDA0003624414630000107
为横向坐标系北向速度，
Figure BDA0003624414630000108
为横向坐标系东向速度，
Figure BDA0003624414630000109
为横向坐标系速度，Δtg为GPS采样时间间隔，Δts为SINS的采样时间间隔，且有
Figure BDA00036244146300001010
G为GPS采样间隔和SINS采样间隔的关系系数，
Figure BDA00036244146300001011
为正实数集合，tM为GPS的采样时间，M为更新时间间隔的次数，tM＝t＝MΔtg，Δtg＝tk+1-tk，k＝0,1，2，3，...,M-1。请参考图5观测矢量和改进观测矢量的对比图。
步骤S203：建立横向系下无迹卡尔曼滤波的预测模型和量测模型；
所述无迹卡尔曼滤波的预测模型为：
Figure BDA00036244146300001012
其中，
Figure BDA00036244146300001013
为陀螺仪的输出，εb(t)为陀螺零偏，η为量测噪声，是均值为零的高斯白噪声，
Figure BDA00036244146300001014
为微分后的陀螺零偏，
Figure BDA00036244146300001015
为横向坐标系姿态方向余弦矩阵，
Figure BDA00036244146300001016
为横向坐标系相对于惯性系的角速度，
Figure BDA0003624414630000111
为横向坐标系姿态方向余弦矩阵
Figure BDA0003624414630000112
的矩阵转置。
将所述无迹卡尔曼滤波的预测模型转换为四元数表示的预测模型，其表达式为：
Figure BDA0003624414630000113
其中，
Figure BDA0003624414630000114
为横向坐标系k-1时刻的姿态四元数，εk-1为k-1时刻的陀螺零偏，
Figure BDA0003624414630000115
为陀螺在b系中的测量角速度，
Figure BDA0003624414630000116
为横向坐标系相对于惯性系的角速度，b为载体坐标系，h为用四元数表示的离散时间姿态运动学方程。
所述无迹卡尔曼滤波的量测模型为：
Figure BDA0003624414630000117
Figure BDA0003624414630000118
其中，
Figure BDA0003624414630000119
为
Figure BDA00036244146300001110
的方向余弦矩阵，
Figure BDA00036244146300001111
为tM时刻的横向坐标系姿态四元数，tM为GPS的采样时间，M为更新时间间隔的次数，η为量测噪声，T为矩阵转置。
步骤S204：构建抑制GPS野值的模值匹配权值函数；
建立实际速度测量值与理想值的关系式：
Figure BDA00036244146300001112
Figure BDA00036244146300001113
将所述实际速度测量值与理想值的关系式代入
Figure BDA00036244146300001114
中计算得到
Figure BDA00036244146300001115
利用Huber鲁棒滤波理论中幅值匹配方法，计算得到权值函数：
Figure BDA00036244146300001116
其中，
Figure BDA00036244146300001117
为实际测量速度，
Figure BDA00036244146300001118
为真实速度，
Figure BDA00036244146300001119
为tM时刻高斯噪声与野值的混合噪声，
Figure BDA00036244146300001120
为实际观测矢量，βM为无误差观测矢量，γ为高斯阈值，ξM为归一化残差，其表达式为：
Figure BDA00036244146300001121
Figure BDA0003624414630000121
σ为随机变量方差，var[εM]为εM的方差，εM为归一化残差。
当GPS输出速度服从高斯分布时，满足
Figure BDA0003624414630000122
I3为单位矩阵，则实际观测矢量
Figure BDA0003624414630000123
与参考矢量模值平方的残差可以重写为：
Figure BDA0003624414630000124
步骤S205：结合无迹四元数卡尔曼滤波模型与权值函数推导了横向系下的鲁棒无迹四元数估计器，以修正罗德里格参数和陀螺零偏作为状态向量；
所述横向系鲁棒无迹四元数卡尔曼估计器模型：
Figure BDA0003624414630000125
Figure BDA0003624414630000126
其中，
Figure BDA0003624414630000127
为改进后的观测矢量，
Figure BDA0003624414630000128
为改进前的实际观测矢量，
Figure BDA0003624414630000129
为量测预测的均值，
Figure BDA00036244146300001210
为量测预测的均值。。
若有横向误差四元数为δq＝[δρT δq4]T，则对应的MRP为
Figure BDA00036244146300001211
其中a为一个取值范围为0到1的常数，δρ为四元数矢量部分，δq4为四元数标量，f是一个比例因子。
状态向量：xk＝[δpk；εk]
步骤S206：初始化横向系鲁棒无迹四元数卡尔曼估计器模型；
设k-1时刻的状态最优估计向量为
Figure BDA00036244146300001212
相应的状态协方差矩阵为Pk-1,并且最优估计四元数为
Figure BDA00036244146300001213
步骤S207：利用预测模型进行预测更新；
先计算Sigma点集χk-1
Figure BDA0003624414630000131
式中，n为状态向量的维数，k为满足加权和的系数，通常k＝3-n。
Figure BDA0003624414630000132
向量矩阵中其中第i(i∈[1,2n+1])个向量
Figure BDA0003624414630000133
的误差四元数
Figure BDA0003624414630000134
可以由下式计算得到：
Figure BDA0003624414630000135
δρ＝f-1[a+δq4]δp
δq4为误差四元数标量部分，δρ为误差四元数矢量部分。对应的四元数的Sigma点集
Figure BDA0003624414630000136
为：
Figure BDA0003624414630000137
将姿态方向余弦矩阵以四元数的形式和陀螺零偏的Sigma点集代入式
Figure BDA0003624414630000138
的姿态更新方程(预测模型)得到预测四元数Sigma点集
Figure BDA0003624414630000139
Figure BDA00036244146300001310
为预测四元Sigma点集，由于陀螺零偏可视为常值，所以预测陀螺零偏Sigma点集为
Figure BDA00036244146300001311
预测误差四元数Sigma点集为:
Figure BDA00036244146300001312
因为
Figure BDA00036244146300001313
对应的预测MRP的Sigma点集为
Figure BDA00036244146300001314
由于
Figure BDA0003624414630000141
所以预测的状态向量均值与状态协方差由下式可得：
Figure BDA0003624414630000142
Figure BDA0003624414630000143
式中W(i)为第i个加权系数，取值为:
Figure BDA0003624414630000144
Figure BDA0003624414630000145
Qk-1为协方差矩阵的过程噪声矩阵。
步骤S208：利用量测模型进行量测更新；
将预测四元数Sigma点集代入量测方程得到量测预测的Sigma点集为：
Figure BDA0003624414630000146
量测预测的均值和协方差矩阵为：
Figure BDA0003624414630000147
Figure BDA0003624414630000148
其中Rk为量测噪声矩阵。互相关协方差矩阵为：
Figure BDA0003624414630000149
滤波增益为：
Kk＝Pxy,k(Py,k)-1
结合权值函数，改进的观测向量由
Figure BDA00036244146300001410
变为
Figure BDA00036244146300001411
Figure BDA00036244146300001412
Figure BDA0003624414630000151
为改进横向坐标系观测矢量，
Figure BDA0003624414630000152
为横向坐标系观测矢量。状态估计向量为
Figure BDA0003624414630000153
状态估计协方差阵：
Pk＝Pk|k-1-KkPy,kKk T
其中，Kk T为滤波增益Kk的矩阵转置。
步骤S209：进行姿态更新完成粗对准，将粗对准结果上传显示器。
Figure BDA0003624414630000154
误差四元数
Figure BDA0003624414630000155
最优四元数估计为：
Figure BDA0003624414630000156
将状态估计向量
Figure BDA0003624414630000157
的前三个值即MRP置0再进行下一轮估计。直至初始粗对准过程结束，图6为陀螺零偏估计曲线图，图7、图8和图9为本发明方法得到的横向系俯仰角、横滚角、航向角的误差曲线图。
在本实施例中详细公开了一种基于鲁棒无迹四元数卡尔曼滤波的极区粗对准方法实现了极区行进间粗对准。本发明包括：依据横向系力学编排，构建横向系下GPS辅助的行进间初始对准的观测矢量模型；建立横向系下无迹卡尔曼滤波的预测模型和量测模型，分析GPS野值对观测矢量的影响；结合无迹四元数卡尔曼滤波和权值函数设计了横向系下鲁棒无迹四元数估计器，并采用修正罗德里格参数和陀螺零偏作为状态向量，以GPS输出速度转横向系后作为观测量，估计出初始姿态，完成极区初始粗对准。
请参考图10，图10为本发明实施例提供的一种基于鲁棒无迹四元数卡尔曼滤波的极区粗对准的装置的结构框图；具体装置可以包括：
采集模块100，用于采集惯性传感器数据和GPS速度数据；
构建矢量模块200，用于根据横向坐标系的比力方程和所述GPS速度数据构建横向坐标系下的参考矢量和观测矢量；
构建模型模块300，用于根据惯性传感器的数据、所述GPS速度数据以及所述横向坐标系下的参考矢量和观测矢量构建无迹卡尔曼滤波模型；
构建权值函数模块400，用于基于实际速度测量值与理想值得关系构建抑制GPS野值的模值匹配权值函数；
构建估计器模块500，用于基于所述无迹四元数卡尔曼滤波模型与所述抑制GPS野值的模值匹配权值函数构建横向系鲁棒无迹四元数卡尔曼估计器模型；
姿态估计模块600，用于利用所述横向系鲁棒无迹四元数卡尔曼估计器模型进行极区初始姿态估计，完成粗对准过程。
本实施例的一种基于鲁棒无迹四元数卡尔曼滤波的极区粗对准的装置用于实现前述的一种基于鲁棒无迹四元数卡尔曼滤波的极区粗对准方法，因此一种基于鲁棒无迹四元数卡尔曼滤波的极区粗对准的装置中的具体实施方式可见前文中的一种基于鲁棒无迹四元数卡尔曼滤波的极区粗对准方法的实施例部分，例如，采集模块100，构建矢量模块200，构建模型模块300，构建权值函数模块400，构建估计器模块500，姿态估计模块600，分别用于实现上述一种基于鲁棒无迹四元数卡尔曼滤波的极区粗对准方法中步骤S 101，S102，S103，S104，S105和S106所以，其具体实施方式可以参照相应的各个部分实施例的描述，在此不再赘述。
本发明具体实施例还提供了一种基于鲁棒无迹四元数卡尔曼滤波的极区粗对准的设备，包括：存储器，用于存储计算机程序；处理器，用于执行所述计算机程序时实现上述一种基于鲁棒无迹四元数卡尔曼滤波的极区粗对准方法的步骤。
本发明具体实施例还提供了一种计算机可读存储介质，所述计算机可读存储介质上存储有计算机程序，所述计算机程序被处理器执行时实现上述一种基于鲁棒无迹四元数卡尔曼滤波的极区粗对准方法的步骤。
本说明书中各个实施例采用递进的方式描述，每个实施例重点说明的都是与其它实施例的不同之处，各个实施例之间相同或相似部分互相参见即可。对于实施例公开的装置而言，由于其与实施例公开的方法相对应，所以描述的比较简单，相关之处参见方法部分说明即可。
专业人员还可以进一步意识到，结合本文中所公开的实施例描述的各示例的单元及算法步骤，能够以电子硬件、计算机软件或者二者的结合来实现，为了清楚地说明硬件和软件的可互换性，在上述说明中已经按照功能一般性地描述了各示例的组成及步骤。这些功能究竟以硬件还是软件方式来执行，取决于技术方案的特定应用和设计约束条件。专业技术人员可以对每个特定的应用来使用不同方法来实现所描述的功能，但是这种实现不应认为超出本发明的范围。
结合本文中所公开的实施例描述的方法或算法的步骤可以直接用硬件、处理器执行的软件模块，或者二者的结合来实施。软件模块可以置于随机存储器(RAM)、内存、只读存储器(ROM)、电可编程ROM、电可擦除可编程ROM、寄存器、硬盘、可移动磁盘、CD-ROM、或技术领域内所公知的任意其它形式的存储介质中。
以上对本发明所提供的一种基于鲁棒无迹四元数卡尔曼滤波的极区粗对准方法以及装置进行了详细介绍。本文中应用了具体个例对本发明的原理及实施方式进行了阐述，以上实施例的说明只是用于帮助理解本发明的方法及其核心思想。应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以对本发明进行若干改进和修饰，这些改进和修饰也落入本发明权利要求的保护范围内。

Claims (10)

1.一种基于鲁棒无迹四元数卡尔曼滤波的极区粗对准方法，其特征在于，包括：
获取惯性传感器数据和GPS速度数据；
根据横向坐标系的比力方程和所述GPS速度数据构建横向坐标系下的参考矢量和观测矢量；
根据所述惯性传感器的数据、所述GPS速度数据以及所述横向坐标系下的参考矢量和观测矢量构建无迹卡尔曼滤波模型；
基于实际速度测量值与理想值得关系构建抑制GPS野值的模值匹配权值函数；
基于所述无迹四元数卡尔曼滤波模型与所述抑制GPS野值的模值匹配权值函数构建横向系鲁棒无迹四元数卡尔曼估计器模型；
利用所述横向系鲁棒无迹四元数卡尔曼估计器模型进行极区初始姿态估计，完成粗对准过程。
2.如权利要求1所述的方法，其特征在于，所述采集惯性传感器数据和GPS速度数据包括：
采集所述惯性传感器中的陀螺输出值和加速度计输出值，采集GPS输出速度值；
将所述陀螺输出值、所述加速度计输出值和所述GPS输出速度值由传统坐标系转化为横向坐标系。
3.如权利要求1所述的方法，其特征在于，所述根据横向坐标系的比力方程和所述GPS速度数据构建参考矢量和观测矢量包括：
根据采集的所述惯性传感器数据、所述GPS速度数据以及横向坐标系的比力方程构建横向坐标系下参考矢量和观测矢量；
所述参考矢量为：
Figure FDA0003624414620000011
所述观测矢量为：
Figure FDA0003624414620000021
其中，αM为横向坐标系下参考矢量，βM为横向坐标系下观测矢量，fb为采集的传感器加速度计数据，b(0)为初始对准起始时刻载体坐标系，并且相对惯性系不变，b(tk)为tk时刻的载体坐标系，
Figure FDA0003624414620000022
为b系姿态矩阵，nt(0)为初始对准起始时刻横向坐标系，并且相对惯性系不变，nt(tk)为tk时刻的横向导坐标系，
Figure FDA0003624414620000023
为nt系姿态矩阵，nt为横向坐标系，
Figure FDA0003624414620000024
为nt系下得重力加速度矢量，
Figure FDA0003624414620000025
为nt系下et系相对于i系的角速度，et为横向地球坐标系，i为惯性坐标系，ωie为地球自转角速度，Lt为横向纬度，λt为横向经度，
Figure FDA0003624414620000026
为横向坐标系下北向速度，
Figure FDA0003624414620000027
为横向坐标系下东向速度，
Figure FDA0003624414620000028
为横向坐标系下速度，Δtg为GPS采样时间间隔，Δts为SINS的采样时间间隔，且有
Figure FDA0003624414620000029
Figure FDA00036244146200000210
G为GPS采样间隔和SINS采样间隔的关系系数，
Figure FDA00036244146200000211
为正实数集合，tM为GPS的采样时间，M为更新时间间隔的次数，tM＝t＝MΔtg，Δtg＝tk+1-tk，k＝0,1，2，3，...,M-1。
4.如权利要求3所述的方法，其特征在于，所述无迹卡尔曼滤波模型包括预测模型和量测模型；
所述无迹卡尔曼滤波的预测模型为：
Figure FDA0003624414620000031
其中，
Figure FDA0003624414620000032
为陀螺仪的输出，εb(t)为陀螺零偏，η为量测噪声，是均值为零的高斯白噪声，
Figure FDA0003624414620000033
为微分后的陀螺零偏，
Figure FDA0003624414620000034
为横向坐标系姿态方向余弦矩阵，
Figure FDA0003624414620000035
为横向坐标系相对于惯性系的角速度，
Figure FDA0003624414620000036
为横向坐标系姿态方向余弦矩阵
Figure FDA0003624414620000037
的矩阵转置。
将所述无迹卡尔曼滤波的预测模型转换为四元数表示的预测模型，其表达式为：
Figure FDA0003624414620000038
其中，
Figure FDA0003624414620000039
为横向坐标系k-1时刻的姿态四元数，εk-1为k-1时刻的陀螺零偏，
Figure FDA00036244146200000310
为陀螺在b系中的测量角速度，
Figure FDA00036244146200000311
为横向坐标系相对于惯性系的角速度，b为载体坐标系，h为用四元数表示的离散时间姿态运动学方程。
所述无迹卡尔曼滤波的量测模型为：
Figure FDA00036244146200000312
Figure FDA00036244146200000313
其中，
Figure FDA0003624414620000041
为
Figure FDA0003624414620000042
的方向余弦矩阵，
Figure FDA0003624414620000043
为tM时刻的横向坐标系姿态四元数，tM为GPS的采样时间，M为更新时间间隔的次数，η为量测噪声，T为矩阵转置。
5.如权利要求4所述的方法，其特征在于，所述基于实际速度测量值与理想值得关系构建抑制GPS野值的模值匹配权值函数包括：
建立实际速度测量值与理想值的关系式：
Figure FDA0003624414620000044
Figure FDA0003624414620000045
将所述实际速度测量值与理想值的关系式代入所述量测模型得到
Figure FDA0003624414620000046
利用Huber鲁棒滤波理论中幅值匹配方法，计算得到所述抑制GPS野值的模值匹配权值函数：
Figure FDA0003624414620000047
其中，
Figure FDA0003624414620000048
为横向坐标系tM实际测量速度，
Figure FDA0003624414620000049
为横向坐标系tM真实速度，
Figure FDA00036244146200000410
为tM时刻高斯噪声与野值的混合噪声，
Figure FDA00036244146200000411
为实际观测矢量，βM为无误差观测矢量，γ为高斯阈值，ξM为归一化残差，其表达式为：
Figure FDA00036244146200000412
σ为随机变量方差，var[εM]为εM的方差，εM为归一化残差。
6.如权利要求1所述的方法，其特征在于，所述基于所述无迹四元数卡尔曼滤波模型与所述抑制GPS野值的模值匹配权值函数构建横向系鲁棒无迹四元数卡尔曼估计器模型包括：
根据所述基于所述无迹四元数卡尔曼滤波的预测模型和量测模型以及所述GPS野值的模值匹配权值函数构建横向系鲁棒无迹四元数卡尔曼估计器模型：
Figure FDA0003624414620000051
Figure FDA0003624414620000052
其中，
Figure FDA0003624414620000053
为改进后的观测矢量，
Figure FDA0003624414620000054
为改进前的实际观测矢量，
Figure FDA0003624414620000055
为量测预测的均值。
7.如权利要求6所述的方法，其特征在于，所述利用所述横向系鲁棒无迹四元数卡尔曼估计器模型进行极区初始姿态估计，完成粗对准过程包括：
初始化算法，给定k-1时刻的状态的估计向量
Figure FDA0003624414620000056
状态协方差矩阵Pk-1，估计四元数
Figure FDA0003624414620000057
利用所述估计向量
Figure FDA0003624414620000058
和所述状态协方差矩阵Pk-1计算Sigma点集χk-1；
根据所述Sigma点集χk-1和所述状态向量计算得到四元数的Sigma点集
Figure FDA0003624414620000059
基于所述四元数预测模型，计算得到预测四元数Sigma点集和预测误差四元数Sigma点集，计算预测更新的状态向量与状态协方差矩阵；
将所述预测四元数Sigma点集输入所述量测模型中计算得到量测预测Sigma点集、量测预测的均值和协方差矩阵；
根据所述协方差矩阵计算滤波增益；
基于所述滤波增益和所述抑制GPS野值的模值匹配权值函数，计算观测向量和状态估计向量，并更新状态估计协方差阵。
8.一种基于鲁棒无迹四元数卡尔曼滤波的极区粗对准的装置，其特征在于，包括：
获取模块，用于获取惯性传感器数据和GPS速度数据；
构建矢量模块，用于根据横向坐标系的比力方程和所述GPS速度数据构建横向坐标系下的参考矢量和观测矢量；
构建模型模块，用于根据惯性传感器的数据、所述GPS速度数据以及所述横向坐标系下的参考矢量和观测矢量构建无迹卡尔曼滤波模型；
构建权值函数模块，用于基于实际速度测量值与理想值得关系构建抑制GPS野值的模值匹配权值函数；
构建估计器模块，用于基于所述无迹四元数卡尔曼滤波模型与所述抑制GPS野值的模值匹配权值函数构建横向系鲁棒无迹四元数卡尔曼估计器模型；
姿态估计模块，用于利用所述横向系鲁棒无迹四元数卡尔曼估计器模型进行极区初始姿态估计，完成粗对准过程。
9.一种极区定位系统，其特征在于，包括：
惯性传感器，安装于被定位载体上，用于采集载体的陀螺输出值和加速度计输出值；
GPS定位器，安装于所述极区定位系统中，用于定位所述载体的速度数据；
处理器，用于执行上述权利要求1-7任一项所述的基于鲁棒无迹四元数卡尔曼滤波的极区粗对准方法的步骤，完成极区粗对准，并将数据上传；
控制显示器：用于接收并显示所述处理器上传的极区粗对准的定位结果。
10.一种计算机可读存储介质，其特征在于，所述计算机可读存储介质上存储有计算机程序，所述计算机程序被处理器执行时实现如权利要求1至7任一项所述一种基于鲁棒无迹四元数卡尔曼滤波的极区粗对准方法的步骤。
CN202210466480.5A 2022-04-29 2022-04-29 一种基于鲁棒无迹四元数卡尔曼滤波的极区粗对准方法 Pending CN114812614A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202210466480.5A CN114812614A (zh) 2022-04-29 2022-04-29 一种基于鲁棒无迹四元数卡尔曼滤波的极区粗对准方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202210466480.5A CN114812614A (zh) 2022-04-29 2022-04-29 一种基于鲁棒无迹四元数卡尔曼滤波的极区粗对准方法

Publications (1)

Publication Number Publication Date
CN114812614A true CN114812614A (zh) 2022-07-29

Family

ID=82510195

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202210466480.5A Pending CN114812614A (zh) 2022-04-29 2022-04-29 一种基于鲁棒无迹四元数卡尔曼滤波的极区粗对准方法

Country Status (1)

Country Link
CN (1) CN114812614A (zh)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN116608859A (zh) * 2023-05-17 2023-08-18 南京信息工程大学 一种基于阈值处理的自适应无迹卡尔曼滤波的导航方法、存储介质和设备

Citations (9)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN101788679A (zh) * 2010-02-08 2010-07-28 北京航空航天大学 一种基于新息正交的sins/gps自适应野值检测与实时补偿方法
CN109708670A (zh) * 2019-03-08 2019-05-03 哈尔滨工程大学 基于改进的Sage-Husa自适应卡尔曼滤波的极区传递对准挠曲变形的噪声补偿方法
CN110109470A (zh) * 2019-04-09 2019-08-09 西安电子科技大学 基于无迹卡尔曼滤波的联合定姿方法、卫星姿态控制系统
WO2020087845A1 (zh) * 2018-10-30 2020-05-07 东南大学 基于gpr与改进的srckf的sins初始对准方法
AU2020101268A4 (en) * 2020-07-06 2020-08-13 Harbin Engineering University The initial alignment method for sway base
AU2020101525A4 (en) * 2020-07-28 2020-09-03 Xi`an University of Architecture and Technology Inertial basis combined navigation method for indirect grid frame in polar zone
CN111896008A (zh) * 2020-08-20 2020-11-06 哈尔滨工程大学 一种改进的鲁棒无迹卡尔曼滤波组合导航方法
CN111947685A (zh) * 2020-07-30 2020-11-17 苏州大学 一种极区格网坐标系动基座粗对准方法
CN114234970A (zh) * 2021-12-21 2022-03-25 华南农业大学 一种运动载体姿态实时测量方法、装置

Patent Citations (9)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN101788679A (zh) * 2010-02-08 2010-07-28 北京航空航天大学 一种基于新息正交的sins/gps自适应野值检测与实时补偿方法
WO2020087845A1 (zh) * 2018-10-30 2020-05-07 东南大学 基于gpr与改进的srckf的sins初始对准方法
CN109708670A (zh) * 2019-03-08 2019-05-03 哈尔滨工程大学 基于改进的Sage-Husa自适应卡尔曼滤波的极区传递对准挠曲变形的噪声补偿方法
CN110109470A (zh) * 2019-04-09 2019-08-09 西安电子科技大学 基于无迹卡尔曼滤波的联合定姿方法、卫星姿态控制系统
AU2020101268A4 (en) * 2020-07-06 2020-08-13 Harbin Engineering University The initial alignment method for sway base
AU2020101525A4 (en) * 2020-07-28 2020-09-03 Xi`an University of Architecture and Technology Inertial basis combined navigation method for indirect grid frame in polar zone
CN111947685A (zh) * 2020-07-30 2020-11-17 苏州大学 一种极区格网坐标系动基座粗对准方法
CN111896008A (zh) * 2020-08-20 2020-11-06 哈尔滨工程大学 一种改进的鲁棒无迹卡尔曼滤波组合导航方法
CN114234970A (zh) * 2021-12-21 2022-03-25 华南农业大学 一种运动载体姿态实时测量方法、装置

Non-Patent Citations (4)

* Cited by examiner, † Cited by third party
Title
XIAOREN ZHOU: "A Robust Quaternion Kalman Filter Method for MIMU/GPS In-Motion Alignment", IEEE TRANSACTIONS ON INSTRUMENTATION AND MEASUREMENT, vol. 70, 31 December 2021 (2021-12-31) *
周晓仁: "鲁棒无迹四元数卡尔曼滤波初始对准算法", 传感技术学报, vol. 34, no. 12, 31 December 2021 (2021-12-31), pages 1622 - 1630 *
张秋昭;张书毕;郑南山;王坚;: "GPS/INS组合系统的多重渐消鲁棒容积卡尔曼滤波", 中国矿业大学学报, no. 01, 26 January 2014 (2014-01-26) *
李京书;郭士荦;崔国恒;陈重阳;: "捷联惯导系统极区水下动基座对准", 仪器仪表学报, no. 10, 15 October 2018 (2018-10-15) *

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN116608859A (zh) * 2023-05-17 2023-08-18 南京信息工程大学 一种基于阈值处理的自适应无迹卡尔曼滤波的导航方法、存储介质和设备

Similar Documents

Publication Publication Date Title
CN113465628B (zh) 惯性测量单元数据补偿方法及系统
RU2701194C2 (ru) Способ оценки навигационного состояния в условиях ограниченной возможности наблюдения
Chatzi et al. The unscented Kalman filter and particle filter methods for nonlinear structural system identification with non‐collocated heterogeneous sensing
US9062978B2 (en) Tracking a body by nonlinear and non-Gaussian parametric filtering
CN111156987A (zh) 基于残差补偿多速率ckf的惯性/天文组合导航方法
CN110118560A (zh) 一种基于lstm和多传感器融合的室内定位方法
CN110715659A (zh) 零速检测方法、行人惯性导航方法、装置及存储介质
CN111721288A (zh) 一种mems器件零偏修正方法、装置及存储介质
CN112525218A (zh) 一种ins/dvl组合导航系统鲁棒智能协同校准方法
US20140222369A1 (en) Simplified method for estimating the orientation of an object, and attitude sensor implementing such a method
CN105424036A (zh) 一种低成本水下潜器地形辅助惯性组合导航定位方法
CN104655131A (zh) 基于istssrckf的惯性导航初始对准方法
CN107479076B (zh) 一种动基座下联合滤波初始对准方法
CN109141475A (zh) 一种dvl辅助sins鲁棒行进间初始对准方法
CN120125628B (zh) 一种基于bim模型与ar实景的几何配准方法及系统
CN104374405A (zh) 一种基于自适应中心差分卡尔曼滤波的mems捷联惯导初始对准方法
CN103557864A (zh) Mems捷联惯导自适应sckf滤波的初始对准方法
CN115420291B (zh) 一种大范围室内场景下的多源融合定位方法及装置
CN103674064B (zh) 捷联惯性导航系统的初始标定方法
CN116448111A (zh) 一种基于多源信息融合的行人室内导航方法、装置及介质
CN106370178A (zh) 移动终端设备的姿态测量方法及装置
CN104613966B (zh) 一种地籍测量事后数据处理方法
CN116007620A (zh) 一种组合导航滤波方法、系统、电子设备及存储介质
CN111504278A (zh) 基于自适应频域积分的海浪检测方法
CN108507571B (zh) 一种高速运动学下imu姿态捕捉方法及系统

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination