CN112629538A - 基于融合互补滤波和卡尔曼滤波的舰船水平姿态测量方法 - Google Patents

基于融合互补滤波和卡尔曼滤波的舰船水平姿态测量方法 Download PDF

Info

Publication number
CN112629538A
CN112629538A CN202011462399.7A CN202011462399A CN112629538A CN 112629538 A CN112629538 A CN 112629538A CN 202011462399 A CN202011462399 A CN 202011462399A CN 112629538 A CN112629538 A CN 112629538A
Authority
CN
China
Prior art keywords
navigation
attitude
quaternion
coordinate system
ship
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.)
Granted
Application number
CN202011462399.7A
Other languages
English (en)
Other versions
CN112629538B (zh
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.)
Harbin Engineering University
Original Assignee
Harbin Engineering 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 Harbin Engineering University filed Critical Harbin Engineering University
Priority to CN202011462399.7A priority Critical patent/CN112629538B/zh
Publication of CN112629538A publication Critical patent/CN112629538A/zh
Application granted granted Critical
Publication of CN112629538B publication Critical patent/CN112629538B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/20—Instruments for performing navigational calculations
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
    • G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
    • G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
    • G01S19/42—Determining position
    • G01S19/45—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement
    • G01S19/47—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement the supplementary measurement being an inertial measurement, e.g. tightly coupled inertial

Abstract

本发明属于舰船导航制导与控制技术领域，具体涉及一种基于融合互补滤波和卡尔曼滤波的舰船水平姿态测量方法。本发明利用实时采集的微机电惯性测量单元陀螺仪输出信号和加速度计输出信号进行捷联惯性导航解算，综合利用各传感器的优点，实现了系统高精度的水平姿态测量。本发明通过卡尔曼滤波后的导航参数对机动状态下载体的线加速度和哥氏加速度进行补偿，并采用互补滤波补偿陀螺积分误差，使水平姿态保持较高精度输出，即使系统存在运动线加速度时，依然保证互补滤波的效果以及失准角的最优计算，有效的提高了系统姿态测量精度，具有一定的工程应用价值。

Description

基于融合互补滤波和卡尔曼滤波的舰船水平姿态测量方法
技术领域
本发明属于舰船导航制导与控制技术领域，具体涉及一种基于融合互补滤波和卡尔曼滤波的舰船水平姿态测量方法。
背景技术
随着微机电系统技术的发展，低成本MEMS IMU在导航领域有着越来越多的应用，通过利用基于微机电系统的惯性传感器进行运动参数测量，可以检测船舶在海中复杂的运动状态，实时输出以四元数、欧拉角等表示的载体姿态角和线性位移数据，从而实现用户对水面舰船的运动数据采集。
微机电陀螺仪具有随机漂移特性，其积分误差随时间累积，加速度计不存在累积误差，但是易受到载体震动影响。常用的将二者数据融合的算法是卡尔曼滤波和互补滤波，例如在专利申请号为201811070907.X,名称为“基于机动状态判断的MEMS惯性导航系统水平姿态自修正方法”的专利文件中，通过比较加速度计输出和当地重力加速度幅值，将载体运动分为低，中，高动态。在低和中动态时，实时调整量测噪声矩阵，在高动态时只进行时间更新。又如在专利申请号为201911277173.7，名称为“一种高精度无人机系统及智能控制方法”的专利文件中，基于GPS模块接受卫星信号的精度因子和速度信息对互补滤波器的截止频率建立了自适应函数来满足不同运动状态下的解算精度。又如在专利申请号为201810875386.9,名称为“一种基于单天线GPS和IMU的大机动条件下无人机姿态估计方法”的专利文件中，将GPS测得的速度值差分，得到的结果对IMU测得的加速度数据进行补偿，再利用修正后的加速度信息修正陀螺仪。
发明内容
本发明的目的在于提供一种基于融合互补滤波和卡尔曼滤波的舰船水平姿态测量方法。
本发明的目的通过如下技术方案来实现：包括以下步骤：
步骤1：获取初始捷联惯性导航系统的导航参数；
步骤2：采集加速度计输出的比力fb和陀螺仪输出的角速度
Figure BDA0002832024950000011
步骤3：进行捷联惯性导航系统初始对准，得到载体系相对于导航坐标系的初始捷联姿态矩阵
Figure BDA0002832024950000012
b系代表载体系；n系代表导航坐标系，取地理坐标系作为导航坐标系；
步骤4：根据初始捷联姿态矩阵
Figure BDA0002832024950000013
和采集到的角速度
Figure BDA0002832024950000014
递推更新得到姿态四元数并对其进行归一化处理，归一化后的姿态四元数q为：
q＝[q1 q2 q3 q4]T
其中，q1、q2、q3和q4是归一化后四元数q的元素；
步骤5：根据归一化后的姿态四元数q更新捷联姿态矩阵
Figure BDA0002832024950000021
Figure BDA0002832024950000022
步骤6：利用捷联姿态矩阵
Figure BDA0002832024950000023
将采集到的加速度计输出的比力fb转换到导航坐标系下；
Figure BDA0002832024950000024
其中，fn表示加速度计输出比力fb在导航坐标系的投影；
步骤7：根据加速度计输出比力fb在导航坐标系的投影fn去除有害加速度后得到舰船的加速度，并计算得到舰船的速度Vn(t)；
Figure BDA0002832024950000025
其中，
Figure BDA0002832024950000026
为舰船的东向速度；
Figure BDA0002832024950000027
为舰船的北向速度；
Figure BDA0002832024950000028
为舰船的天向速度；t表示当前时刻；
步骤8：利用舰船的速度Vn(t)更新捷联惯性导航系统的导航纬度
Figure BDA0002832024950000029
和经度λ(t)；
步骤9：选取位置误差
Figure BDA00028320249500000210
东北天三个方向的速度误差δV＝[δVEδVN δVU]T、平台失准角误差φ＝[φx φy φz]T、载体系三轴的加速度计的零位偏移ΔA＝[ΔAbx ΔAby ΔAbz]T和陀螺仪的常值漂移ε＝[εbx εby εbz]T，作为卡尔曼滤波的状态估计量X＝[δP δV φ ΔA ε]T；
步骤10：选取卡尔曼滤波的系统噪声向量WB为：
WB＝[wax way waz wgx wgy wgz]T
其中，wax、way和waz为捷联惯性导航系统在载体系中三轴加速度计的随机噪声；wgx、wgy和wgz为捷联惯性导航系统在载体系中三轴陀螺仪的随机噪声，均为高斯白噪声；
步骤11：将捷联惯性导航系统导航解算的纬度
Figure BDA00028320249500000211
和经度λ(t)与GPS提供的纬度
Figure BDA00028320249500000212
和经度λA(t)的差值作为卡尔曼滤波的量测值Z(t)，量测值Z(t)和对应量测矩阵H(t)分别为：
Figure BDA0002832024950000031
步骤12：采用卡尔曼滤波算法对选取的状态量进行实时估计，得到捷联惯性导航系统的位置误差
Figure BDA0002832024950000032
速度误差
Figure BDA0002832024950000033
平台失准角的状态估计值
Figure BDA0002832024950000034
步骤13：利用捷联惯性导航系统的位置误差
Figure BDA0002832024950000035
和速度误差
Figure BDA0002832024950000036
校正捷联惯性导航系统解算的位置和速度信息；
Figure BDA0002832024950000037
步骤14：利用捷联惯性导航系统的平台失准角的状态估计值
Figure BDA0002832024950000038
构造四元数
Figure BDA0002832024950000039
对捷联惯性导航系统解算的当前时刻捷联姿态矩阵
Figure BDA00028320249500000310
对应的姿态四元数q＝[q1 q2 q3 q4]T进行补偿得到修正后的四元数q′＝[q′1 q′2 q′3q′4]T；
Figure BDA00028320249500000311
步骤15：将修正后的四元数q′进行归一化处理，得到归一化后的四元数
Figure BDA00028320249500000312
Figure BDA00028320249500000313
步骤16：根据归一化后的姿态四元数
Figure BDA00028320249500000314
计算校正后的捷联姿态矩阵
Figure BDA00028320249500000315
Figure BDA0002832024950000041
步骤17：利用校正后的捷联惯性导航系统的导航参数，借助于比力方程对舰船机动引起的线加速度和哥氏加速度进行补偿，得到重力加速度在导航系的理论输出gn：
Figure BDA0002832024950000042
其中，V1 n为经卡尔曼滤波校正后的地速；
Figure BDA0002832024950000043
为相邻两个姿态解算时刻的速度变化率；
Figure BDA0002832024950000044
为校正后的地球自转角速度在导航坐标系的投影；
Figure BDA0002832024950000045
为舰船的运动引起导航坐标系相当于地球坐标系的旋转角速度在导航坐标系的投影；
步骤18：将重力加速度在导航系的理论输出gn和重力加速度g两矢量归一化并进行叉乘运算得到陀螺积分后的姿态误差
Figure BDA0002832024950000046
Figure BDA0002832024950000047
其中，|| ||表示取向量模值；
步骤19：利用陀螺积分后的姿态误差
Figure BDA0002832024950000048
求取陀螺积分误差Δωb：
Figure BDA0002832024950000049
其中，dt为两个姿态解算时刻差值，kp，ki为比例和积分系数；
步骤20：利用陀螺积分误差Δωb补偿陀螺仪输出的角速度；
Figure BDA00028320249500000410
步骤21：将陀螺仪数据去除地球自转角速度在导航坐标系n的投影以及载体运动导致的导航坐标系n相对地球坐标系e的旋转角速度在导航坐标系n系的投影，得到
Figure BDA00028320249500000411
Figure BDA00028320249500000412
步骤22：利用角速度数据
Figure BDA00028320249500000413
更新修正后的四元数，得到
Figure BDA00028320249500000414
Figure BDA0002832024950000051
步骤23：将四元数
Figure BDA0002832024950000052
进行归一化处理，得到归一化后的四元数
Figure BDA0002832024950000053
Figure BDA0002832024950000054
步骤24：根据归一化后的四元数
Figure BDA0002832024950000055
计算校正后的捷联姿态矩阵
Figure BDA0002832024950000056
完成舰船水平姿态测量的更新和修正；
Figure BDA0002832024950000057
本发明的有益效果在于：
本发明利用实时采集的微机电惯性测量单元陀螺仪输出信号和加速度计输出信号进行捷联惯性导航解算，初步得出运载体位置速度和姿态信息；再利用GPS给出的位置数据作为系统的外界辅助信息，采用间接卡尔曼滤波方法对速度误差，位置误差，平台失准角等状态量进行实时估计，并对初步得出的导航参数进行校正。最后借助比力方程，补偿运载体线加速度以及哥氏加速度得到重力加速度在导航系的理论输出，采用比例积分校正补偿陀螺仪积分误差从而得到更高测量精度的水平姿态信息。本发明综合利用各传感器的优点，实现了系统高精度的水平姿态测量。通过卡尔曼滤波后的导航参数对机动状态下载体的线加速度和哥氏加速度进行补偿，并采用互补滤波补偿陀螺积分误差，使水平姿态保持较高精度输出，即使系统存在运动线加速度时，依然保证互补滤波的效果以及失准角的最优计算，有效的提高了系统姿态测量精度，具有一定的工程应用价值。
附图说明
图1为本发明的架构图。
具体实施方式
下面结合附图对本发明做进一步描述。
本发明属于舰船导航制导与控制技术领域，提供了一种基于融合互补滤波和卡尔曼滤波的舰船水平姿态测量方法，涉及以微机电惯性测量单元为核心器件的姿态测量系统。
本发明首先利用实时采集的微机电惯性测量单元陀螺仪输出信号
Figure BDA0002832024950000058
和加速度计输出信号fb进行捷联惯性导航解算，初步得出运载体位置速度和姿态信息；再利用GPS给出的位置数据作为系统的外界辅助信息，采用间接卡尔曼滤波方法对速度误差，位置误差，平台失准角等状态量进行实时估计，并对初步得出的导航参数进行校正。最后借助比力方程，补偿运载体线加速度以及哥氏加速度得到重力加速度在导航系的理论输出，采用比例积分校正补偿陀螺仪积分误差从而得到更高测量精度的水平姿态信息。本发明综合利用各传感器的优点，同时对机动状态下载体的线加速度进行补偿，保证系统在不同运动状态下均具有较高的姿态测量精度，具有一定的工程应用价值。本发明具体步骤如下：
步骤1、给定初始捷联惯性导航系统的导航参数；
步骤2、采集加速度计输出的比力fb和陀螺仪输出的角速度
Figure BDA0002832024950000061
步骤3、进行捷联惯性导航系统初始对准，得到载体系(b系)相对于导航坐标系(n系，取地理坐标系作为导航坐标系)的初始捷联姿态矩阵
Figure BDA0002832024950000062
步骤4、进行捷联惯性导航解算，得到运载体速度，位置和姿态等导航参数；
步骤5、选取捷联惯导系统解算的位置误差、速度误差、平台失准角误差、三轴加速度计的零位偏移和陀螺仪的常值漂移为卡尔曼滤波的状态估计量；
步骤6、将捷联惯性导航系统导航解算的位置信息(纬度
Figure BDA0002832024950000063
和经度λ(t))与GPS提供的纬度
Figure BDA0002832024950000064
和经度λA(t)的差值作为卡尔曼滤波的量测值Z(t)，量测值Z(t)和对应量测矩阵H(t)分别为：
Figure BDA0002832024950000065
步骤7、采用卡尔曼滤波算法对选取的状态量进行实时估计，得到捷联惯性导航系统的位置误差、速度误差和平台失准角的状态估计值；
步骤8、利用步骤7中估计的状态来校正捷联惯性导航系统得到的导航参数；
步骤9、利用步骤8中校正的捷联惯性导航系统的导航参数和地球曲率半径，地球自转角速度等常量借助于比力方程对运载体机动引起的线加速度和哥氏加速度进行补偿，得到重力加速度在导航系的理论输出gn：
Figure BDA0002832024950000066
其中，V1 n为经卡尔曼滤波校正后的地速，
Figure BDA0002832024950000067
为相邻两个姿态解算时刻的速度变化率，
Figure BDA0002832024950000068
为校正后的地球自转角速度在导航坐标系的投影,
Figure BDA0002832024950000069
为运载体的运动引起导航坐标系相当于地球坐标系(e系)的旋转角速度在导航坐标系的投影，
Figure BDA00028320249500000610
为卡尔曼滤波校正后姿态对应的捷联姿态矩阵，gn为重力加速度在导航系的理论输出，fb为加速度计输出的比力；
步骤10、对步骤9中求得的gn和实际重力加速度g两矢量归一化并进行叉乘运算得陀螺积分后的姿态误差
Figure BDA0002832024950000071
Figure BDA0002832024950000072
其中|| ||表示取向量模值；
步骤11、利用步骤10得到的姿态误差
Figure BDA0002832024950000073
求取陀螺积分误差Δωb：
Figure BDA0002832024950000074
其中dt为两个姿态解算时刻差值，kp，ki为比例和积分系数；比例环节系数取kp＝0.5，积分环节系数取ki＝0.1；
步骤12、利用步骤11中求取的陀螺积分误差Δωb来补偿陀螺仪积分误差；
步骤13、利用步骤12中补偿后的陀螺仪输出更新姿态四元数，从而校正捷联惯性导航系统的捷联姿态矩阵，获得更高的测量精度；
至此就完成了基于融合互补滤波和卡尔曼滤波算法的水面舰船水平姿态测量方法更新和修正。
本发明综合利用各传感器的优点，实现了系统高精度的水平姿态测量。通过卡尔曼滤波后的导航参数对机动状态下载体的线加速度和哥氏加速度进行补偿，并采用互补滤波补偿陀螺积分误差，使水平姿态保持较高精度输出，即使系统存在运动线加速度时，依然保证互补滤波的效果以及失准角的最优计算，有效的提高了系统姿态测量精度，具有一定的工程应用价值。
实施例1：
本发明利用实时采集的微机电惯性测量单元陀螺仪输出信号
Figure BDA0002832024950000075
和加速度计输出信号fb(b表示载体坐标系)进行捷联惯性导航解算，初步得出运载体位置速度和姿态信息；再利用GPS给出的位置数据作为系统的外界辅助信息，采用间接卡尔曼滤波方法对速度误差，位置误差，平台失准角等状态量进行实时估计，并对初步得出的导航参数进行校正。最后借助比力方程，补偿运载体线加速度以及哥氏加速度得到重力加速度在导航系的理论输出，采用比例积分校正补偿陀螺仪积分误差从而得到更高测量精度的水平姿态，具体步骤如下：
步骤1、给定初始捷联惯性导航系统的导航参数；
步骤2、采集加速度计输出的比力fb和陀螺仪输出的角速度
Figure BDA0002832024950000076
步骤3、进行捷联惯性导航系统初始对准，得到载体系(b系)相对于导航坐标系(n系，取地理坐标系作为导航坐标系)的初始捷联姿态矩阵
Figure BDA0002832024950000077
步骤4、根据步骤3中得到的初始捷联姿态矩阵
Figure BDA0002832024950000078
和步骤2中实时采集到的角速度
Figure BDA0002832024950000079
递推更新得到姿态四元数并对其进行归一化处理，归一化后的姿态四元数q具体表达式为：
q＝[q1 q2 q3 q4]T (5)
其中，q1、q2、q3和q4是归一化后四元数q的元素；
步骤5、根据步骤4中得到的归一化后的姿态四元数q更新捷联姿态矩阵
Figure BDA0002832024950000081
Figure BDA0002832024950000082
步骤6、利用步骤5得到的捷联姿态矩阵
Figure BDA0002832024950000083
将步骤2采集到的加速度计输出的比力fb转换到导航坐标系下：
Figure BDA0002832024950000084
其中，fn表示加速度计输出比力在导航坐标系的投影；
步骤7、根据步骤6中转化到导航坐标系的比力fn去除有害加速度后得到系统的加速度，进一步更新计算得到速度，记为Vn(t)：
Figure BDA0002832024950000085
其中，
Figure BDA0002832024950000086
为系统解算的东向速度、
Figure BDA0002832024950000087
为其北向速度、
Figure BDA0002832024950000088
为其天向速度，t表示当前时刻；
步骤8、利用步骤7更新计算后的速度Vn(t)更新捷联惯性导航系统的导航纬度
Figure BDA00028320249500000817
和经度λ(t)；
步骤9、选取位置误差
Figure BDA0002832024950000089
(纬度误差
Figure BDA00028320249500000810
经度误差δλ和高度误差δh)、东北天三个方向的速度误差δV＝[δVE δVN δVU]T、平台失准角误差φ＝[φx φy φz]T、载体系三轴的加速度计的零位偏移ΔA＝[ΔAbx ΔAby ΔAbz]T和陀螺仪的常值漂移ε＝[εbxεby εbz]T为卡尔曼滤波的状态估计量X：
X＝[δP δV φ ΔA ε]T (9)
步骤10、选取卡尔曼滤波的系统噪声向量WB为：
WB＝[wax way waz wgx wgy wgz]T (10)
其中，wax、way和waz为捷联惯性导航系统在载体系中三轴加速度计的随机噪声，wgx、wgy和wgz为捷联惯性导航系统在载体系中三轴陀螺仪的随机噪声，均为高斯白噪声；
步骤11、将捷联惯性导航系统导航解算的位置信息(纬度
Figure BDA00028320249500000811
和经度λ(t))与GPS提供的纬度
Figure BDA00028320249500000812
和经度λA(t)的差值作为卡尔曼滤波的量测值Z(t)，量测值Z(t)和对应量测矩阵H(t)分别为：
Figure BDA00028320249500000813
步骤12、采用卡尔曼滤波算法对选取的状态量进行实时估计，得到捷联惯性导航系统的位置误差
Figure BDA00028320249500000814
速度误差
Figure BDA00028320249500000815
平台失准角的状态估计值
Figure BDA00028320249500000816
步骤13、利用步骤12中估计出来的位置误差
Figure BDA0002832024950000091
速度误差
Figure BDA0002832024950000092
来校正捷联惯性导航系统解算的位置和速度信息；
Figure BDA0002832024950000093
步骤14、利用步骤12实时估计出来的平台失准角
Figure BDA0002832024950000094
来构造四元数
Figure BDA0002832024950000095
对捷联惯性导航系统解算的当前时刻捷联姿态矩阵
Figure BDA0002832024950000096
对应的姿态四元数q＝[q1 q2 q3 q4]T进行补偿得到修正后的四元数q′＝[q′1 q′2 q′3 q′4]T：
Figure BDA0002832024950000097
Figure BDA0002832024950000098
表示四元数乘法。其中，修正后的四元数q′的元素为：
Figure BDA0002832024950000099
步骤15、将步骤14中得到的修正后的四元数q′进行归一化处理：
Figure BDA00028320249500000910
得到归一化后的四元数为：
Figure BDA00028320249500000911
其中，
Figure BDA00028320249500000912
和
Figure BDA00028320249500000913
是归一化后四元数
Figure BDA00028320249500000914
的元素；
步骤16、根据归一化后的姿态四元数
Figure BDA00028320249500000915
计算得到校正后的捷联姿态矩阵
Figure BDA00028320249500000916
Figure BDA00028320249500000917
至此为卡尔曼滤波器对捷联惯性导航系统的导航参数校正的过程；
步骤17、利用步骤13至步骤16中校正的捷联惯性导航系统的导航参数和地球曲率半径，地球自转角速度等常量，借助于比力方程对运载体机动引起的线加速度和哥氏加速度进行补偿，得到重力加速度在导航系的理论输出gn：
Figure BDA00028320249500000918
其中，V1 n为经卡尔曼滤波校正后的地速，
Figure BDA00028320249500000919
为相邻两个姿态解算时刻的速度变化率，
Figure BDA00028320249500000920
为校正后的地球自转角速度在导航坐标系的投影,
Figure BDA00028320249500000921
为运载体的运动引起导航坐标系相当于地球坐标系(e系)的旋转角速度在导航坐标系的投影，
Figure BDA0002832024950000101
为卡尔曼滤波校正后姿态对应的捷联姿态矩阵，gn为重力加速度在导航系的理论输出，fb为加速度计输出的比力；
步骤18、对步骤17中求得的gn和重力加速度g两矢量归一化并进行叉乘运算得陀螺积分后的姿态误差
Figure BDA0002832024950000102
Figure BDA0002832024950000103
其中|| ||表示取向量模值；
步骤19、利用步骤18得到的姿态误差
Figure BDA0002832024950000104
求取陀螺积分误差Δωb：
Figure BDA0002832024950000105
其中dt为两个姿态解算时刻差值，kp，ki为比例和积分系数；比例环节系数取kp＝0.5，积分环节系数取ki＝0.1；
步骤20、利用步骤19计算的陀螺积分误差补偿陀螺仪输出角速度：
Figure BDA0002832024950000106
步骤21、步骤20得到的补偿后的陀螺仪数据去除地球自转角速度在导航坐标系n的投影和载体运动导致的导航坐标系n相对地球坐标系e的旋转角速度在导航坐标系n系的投影：
Figure BDA0002832024950000107
步骤22、利用步骤21得到的角速度数据
Figure BDA0002832024950000108
更新步骤15得到的四元数：
Figure BDA0002832024950000109
步骤23、将步骤22中得到的修正后的四元数q″进行归一化处理：
Figure BDA00028320249500001010
得到归一化后的四元数为：
Figure BDA00028320249500001011
步骤24、根据归一化后的姿态四元数
Figure BDA00028320249500001012
计算得到校正后的捷联姿态矩阵
Figure BDA00028320249500001013
Figure BDA00028320249500001014
至此就完成了基于融合互补滤波和卡尔曼滤波算法的水面舰船水平姿态测量方法更新和修正。
本发明结合互补滤波和卡尔曼滤波器，通过卡尔曼滤波得到的导航参数借助比力方程补偿载体线加速度和哥氏加速度得到理论重力加速度在导航系的投影，再与实际重力加速度叉乘得出偏差，最后通过比例积分校正修正陀螺输出数据，得出校正后的水平姿态。优点是通过与卡尔曼滤波结合的互补滤波有效地补偿载体的线性加速度，在不同运动状态下都能保持水平姿态较高精度输出，有效提高了系统姿态测量精度，具有一定的工程应用价值。
以上所述仅为本发明的优选实施例而已，并不用于限制本发明，对于本领域的技术人员来说，本发明可以有各种更改和变化。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (1)

1.一种基于融合互补滤波和卡尔曼滤波的舰船水平姿态测量方法，其特征在于，包括以下步骤：
步骤1：获取初始捷联惯性导航系统的导航参数；
步骤2：采集加速度计输出的比力fb和陀螺仪输出的角速度
Figure FDA0002832024940000011
步骤3：进行捷联惯性导航系统初始对准，得到载体系相对于导航坐标系的初始捷联姿态矩阵
Figure FDA0002832024940000012
b系代表载体系；n系代表导航坐标系，取地理坐标系作为导航坐标系；
步骤4：根据初始捷联姿态矩阵
Figure FDA0002832024940000013
和采集到的角速度
Figure FDA0002832024940000014
递推更新得到姿态四元数并对其进行归一化处理，归一化后的姿态四元数q为：
q＝[q1 q2 q3 q4]T
其中，q1、q2、q3和q4是归一化后四元数q的元素；
步骤5：根据归一化后的姿态四元数q更新捷联姿态矩阵
Figure FDA0002832024940000015
Figure FDA0002832024940000016
步骤6：利用捷联姿态矩阵
Figure FDA0002832024940000017
将采集到的加速度计输出的比力fb转换到导航坐标系下；
Figure FDA0002832024940000018
其中，fn表示加速度计输出比力fb在导航坐标系的投影；
步骤7：根据加速度计输出比力fb在导航坐标系的投影fn去除有害加速度后得到舰船的加速度，并计算得到舰船的速度Vn(t)；
Figure FDA0002832024940000019
其中，
Figure FDA00028320249400000110
为舰船的东向速度；
Figure FDA00028320249400000111
为舰船的北向速度；
Figure FDA00028320249400000112
为舰船的天向速度；t表示当前时刻；
步骤8：利用舰船的速度Vn(t)更新捷联惯性导航系统的导航纬度
Figure FDA00028320249400000113
和经度λ(t)；
步骤9：选取位置误差
Figure FDA00028320249400000114
东北天三个方向的速度误差δV＝[δVE δVN δVU]T、平台失准角误差φ＝[φx φy φz]T、载体系三轴的加速度计的零位偏移ΔA＝[ΔAbxΔAby ΔAbz]T和陀螺仪的常值漂移ε＝[εbx εby εbz]T，作为卡尔曼滤波的状态估计量X＝[δPδV φ ΔA ε]T；
步骤10：选取卡尔曼滤波的系统噪声向量WB为：
WB＝[wax way waz wgx wgy wgz]T
其中，wax、way和waz为捷联惯性导航系统在载体系中三轴加速度计的随机噪声；wgx、wgy和wgz为捷联惯性导航系统在载体系中三轴陀螺仪的随机噪声，均为高斯白噪声；
步骤11：将捷联惯性导航系统导航解算的纬度
Figure FDA0002832024940000021
和经度λ(t)与GPS提供的纬度
Figure FDA0002832024940000022
和经度λA(t)的差值作为卡尔曼滤波的量测值Z(t)，量测值Z(t)和对应量测矩阵H(t)分别为：
Figure FDA0002832024940000023
步骤12：采用卡尔曼滤波算法对选取的状态量进行实时估计，得到捷联惯性导航系统的位置误差
Figure FDA0002832024940000024
速度误差
Figure FDA0002832024940000025
平台失准角的状态估计值
Figure FDA0002832024940000026
步骤13：利用捷联惯性导航系统的位置误差
Figure FDA0002832024940000027
和速度误差
Figure FDA0002832024940000028
校正捷联惯性导航系统解算的位置和速度信息；
Figure FDA0002832024940000029
步骤14：利用捷联惯性导航系统的平台失准角的状态估计值
Figure FDA00028320249400000210
构造四元数
Figure FDA00028320249400000211
对捷联惯性导航系统解算的当前时刻捷联姿态矩阵
Figure FDA00028320249400000212
对应的姿态四元数q＝[q1 q2 q3 q4]T进行补偿得到修正后的四元数q′＝[q′1 q′2 q′3 q′4]T；
Figure FDA0002832024940000031
步骤15：将修正后的四元数q′进行归一化处理，得到归一化后的四元数
Figure FDA0002832024940000032
Figure FDA0002832024940000033
步骤16：根据归一化后的姿态四元数
Figure FDA0002832024940000034
计算校正后的捷联姿态矩阵
Figure FDA0002832024940000035
Figure FDA0002832024940000036
步骤17：利用校正后的捷联惯性导航系统的导航参数，借助于比力方程对舰船机动引起的线加速度和哥氏加速度进行补偿，得到重力加速度在导航系的理论输出gn：
Figure FDA0002832024940000037
其中，V1 n为经卡尔曼滤波校正后的地速；
Figure FDA0002832024940000038
为相邻两个姿态解算时刻的速度变化率；
Figure FDA0002832024940000039
为校正后的地球自转角速度在导航坐标系的投影；
Figure FDA00028320249400000310
为舰船的运动引起导航坐标系相当于地球坐标系的旋转角速度在导航坐标系的投影；
步骤18：将重力加速度在导航系的理论输出gn和重力加速度g两矢量归一化并进行叉乘运算得到陀螺积分后的姿态误差
Figure FDA00028320249400000311
Figure FDA00028320249400000312
其中，|| ||表示取向量模值；
步骤19：利用陀螺积分后的姿态误差
Figure FDA00028320249400000313
求取陀螺积分误差Δωb：
Figure FDA00028320249400000314
其中，dt为两个姿态解算时刻差值，kp，ki为比例和积分系数；
步骤20：利用陀螺积分误差Δωb补偿陀螺仪输出的角速度；
Figure FDA0002832024940000041
步骤21：将陀螺仪数据去除地球自转角速度在导航坐标系n的投影以及载体运动导致的导航坐标系n相对地球坐标系e的旋转角速度在导航坐标系n系的投影，得到
Figure FDA0002832024940000042
Figure FDA0002832024940000043
步骤22：利用角速度数据
Figure FDA0002832024940000044
更新修正后的四元数，得到
Figure FDA0002832024940000045
Figure FDA0002832024940000046
步骤23：将四元数
Figure FDA0002832024940000047
进行归一化处理，得到归一化后的四元数
Figure FDA0002832024940000048
Figure FDA0002832024940000049
步骤24：根据归一化后的四元数
Figure FDA00028320249400000410
计算校正后的捷联姿态矩阵
Figure FDA00028320249400000411
完成舰船水平姿态测量的更新和修正；
Figure FDA00028320249400000412
CN202011462399.7A 2020-12-11 2020-12-11 基于融合互补滤波和卡尔曼滤波的舰船水平姿态测量方法 Active CN112629538B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202011462399.7A CN112629538B (zh) 2020-12-11 2020-12-11 基于融合互补滤波和卡尔曼滤波的舰船水平姿态测量方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202011462399.7A CN112629538B (zh) 2020-12-11 2020-12-11 基于融合互补滤波和卡尔曼滤波的舰船水平姿态测量方法

Publications (2)

Publication Number Publication Date
CN112629538A true CN112629538A (zh) 2021-04-09
CN112629538B CN112629538B (zh) 2023-02-14

Family

ID=75312371

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202011462399.7A Active CN112629538B (zh) 2020-12-11 2020-12-11 基于融合互补滤波和卡尔曼滤波的舰船水平姿态测量方法

Country Status (1)

Country Link
CN (1) CN112629538B (zh)

Cited By (15)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113175926A (zh) * 2021-04-21 2021-07-27 哈尔滨工程大学 一种基于运动状态监测的自适应水平姿态测量方法
CN113447018A (zh) * 2021-07-06 2021-09-28 北京理工导航控制科技股份有限公司 一种水下惯性导航系统的姿态实时估计方法
CN113701747A (zh) * 2021-07-20 2021-11-26 北京航天控制仪器研究所 一种基于离心机激励的惯性测量系统姿态角误差分离方法
CN114061571A (zh) * 2021-11-12 2022-02-18 同济大学 一种自适应梯度下降惯性测量单元的姿态解算方法及系统
CN114111771A (zh) * 2021-11-25 2022-03-01 九江中船仪表有限责任公司（四四一厂） 一种双轴稳定平台的动态姿态测量方法
CN114111770A (zh) * 2021-11-19 2022-03-01 清华大学 一种水平姿态测量方法、系统、处理设备及存储介质
CN114166216A (zh) * 2021-11-25 2022-03-11 哈尔滨工程大学 一种aru正置与倒置安装的水平姿态测量方法
CN114323007A (zh) * 2021-12-30 2022-04-12 西人马帝言(北京)科技有限公司 一种载体运动状态估计方法及装置
CN114383613A (zh) * 2022-01-18 2022-04-22 广东工业大学 一种基于北斗定向的电力线舞动监测系统和方法
CN114485641A (zh) * 2022-01-24 2022-05-13 武汉梦芯科技有限公司 一种基于惯导卫导方位融合的姿态解算方法及装置
CN114911252A (zh) * 2022-07-15 2022-08-16 北京航天驭星科技有限公司 基于遥测数据确定火箭姿态的方法、装置、设备、介质
CN115031785A (zh) * 2022-06-21 2022-09-09 浙江大学 一种基于多传感器融合技术的软土勘测方法
CN116588261A (zh) * 2023-07-03 2023-08-15 上海新纪元机器人有限公司 座椅主动补偿控制方法及系统
CN116674701A (zh) * 2023-04-26 2023-09-01 上海新纪元机器人有限公司 减摇防晕座椅及其控制方法和系统
CN113776529B (zh) * 2021-09-13 2024-04-19 中国人民解放军海军工程大学 一种基于载体系四元数姿态误差的非线性误差模型

Citations (17)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US6014103A (en) * 1997-05-29 2000-01-11 Lockheed Martin Corporation Passive navigation system
KR20020080829A (ko) * 2001-04-18 2002-10-26 주식회사 네비콤 오차보정시스템을 구비하는 관성측정유닛-지피에스통합시스템과 미지정수 검색범위 축소방법 및 사이클 슬립검출방법, 및 그를 이용한 항체 위치, 속도,자세측정방법
US20050065727A1 (en) * 2003-09-20 2005-03-24 Guohui Hu Low cost multisensor high precision positioning and data integrated method and system thereof
CN101187567A (zh) * 2007-12-18 2008-05-28 哈尔滨工程大学 基于多普勒的光纤陀螺捷联惯导系统初始姿态确定方法
CN101793523A (zh) * 2010-03-10 2010-08-04 北京航空航天大学 一种组合导航和光电探测一体化系统
CN103245357A (zh) * 2013-04-03 2013-08-14 哈尔滨工程大学 一种船用捷联惯导系统二次快速对准方法
CN104374388A (zh) * 2014-11-10 2015-02-25 大连理工大学 一种基于偏振光传感器的航姿测定方法
CN105021192A (zh) * 2015-07-30 2015-11-04 华南理工大学 一种基于零速校正的组合导航系统的实现方法
CN107015560A (zh) * 2017-03-02 2017-08-04 浙江大学 一种基于无人机的光伏阵列的巡检方法
CN107621264A (zh) * 2017-09-30 2018-01-23 华南理工大学 车载微惯性/卫星组合导航系统的自适应卡尔曼滤波方法
CN107941216A (zh) * 2017-09-18 2018-04-20 哈尔滨工程大学 一种基于自适应互补滤波的捷联惯导系统外水平阻尼方法
CN108413960A (zh) * 2018-01-25 2018-08-17 深圳市博安智控科技有限公司 姿态计算方法和相关设备
CN108534783A (zh) * 2018-05-11 2018-09-14 安徽尼古拉电子科技有限公司 一种基于北斗导航技术的飞行器导航方法
CN108759845A (zh) * 2018-07-05 2018-11-06 华南理工大学 一种基于低成本多传感器组合导航的优化方法
CN109001787A (zh) * 2018-05-25 2018-12-14 北京大学深圳研究生院 一种姿态角解算与定位的方法及其融合传感器
CN110031882A (zh) * 2018-08-02 2019-07-19 哈尔滨工程大学 一种基于sins/dvl组合导航系统的外量测信息补偿方法
CN111307114A (zh) * 2019-11-29 2020-06-19 哈尔滨工程大学 基于运动参考单元的水面舰船水平姿态测量方法

Patent Citations (17)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US6014103A (en) * 1997-05-29 2000-01-11 Lockheed Martin Corporation Passive navigation system
KR20020080829A (ko) * 2001-04-18 2002-10-26 주식회사 네비콤 오차보정시스템을 구비하는 관성측정유닛-지피에스통합시스템과 미지정수 검색범위 축소방법 및 사이클 슬립검출방법, 및 그를 이용한 항체 위치, 속도,자세측정방법
US20050065727A1 (en) * 2003-09-20 2005-03-24 Guohui Hu Low cost multisensor high precision positioning and data integrated method and system thereof
CN101187567A (zh) * 2007-12-18 2008-05-28 哈尔滨工程大学 基于多普勒的光纤陀螺捷联惯导系统初始姿态确定方法
CN101793523A (zh) * 2010-03-10 2010-08-04 北京航空航天大学 一种组合导航和光电探测一体化系统
CN103245357A (zh) * 2013-04-03 2013-08-14 哈尔滨工程大学 一种船用捷联惯导系统二次快速对准方法
CN104374388A (zh) * 2014-11-10 2015-02-25 大连理工大学 一种基于偏振光传感器的航姿测定方法
CN105021192A (zh) * 2015-07-30 2015-11-04 华南理工大学 一种基于零速校正的组合导航系统的实现方法
CN107015560A (zh) * 2017-03-02 2017-08-04 浙江大学 一种基于无人机的光伏阵列的巡检方法
CN107941216A (zh) * 2017-09-18 2018-04-20 哈尔滨工程大学 一种基于自适应互补滤波的捷联惯导系统外水平阻尼方法
CN107621264A (zh) * 2017-09-30 2018-01-23 华南理工大学 车载微惯性/卫星组合导航系统的自适应卡尔曼滤波方法
CN108413960A (zh) * 2018-01-25 2018-08-17 深圳市博安智控科技有限公司 姿态计算方法和相关设备
CN108534783A (zh) * 2018-05-11 2018-09-14 安徽尼古拉电子科技有限公司 一种基于北斗导航技术的飞行器导航方法
CN109001787A (zh) * 2018-05-25 2018-12-14 北京大学深圳研究生院 一种姿态角解算与定位的方法及其融合传感器
CN108759845A (zh) * 2018-07-05 2018-11-06 华南理工大学 一种基于低成本多传感器组合导航的优化方法
CN110031882A (zh) * 2018-08-02 2019-07-19 哈尔滨工程大学 一种基于sins/dvl组合导航系统的外量测信息补偿方法
CN111307114A (zh) * 2019-11-29 2020-06-19 哈尔滨工程大学 基于运动参考单元的水面舰船水平姿态测量方法

Non-Patent Citations (5)

* Cited by examiner, † Cited by third party
Title
BEN YUEYANG ET AL.: "Ocean Current Model Using SINS-GPS-DVL Integrated Navigation", 《APPLIED MECHANICS AND MATERIALS》, vol. 316, 10 April 2013 (2013-04-10), pages 419 - 421 *
YUE YANG ET AL.: "A Nonlinear Double Model for Multisensor-Integrated Navigation Using the Federated EKF Algorithm for Small UAVs：web of science integrated kalman strapdown GPS", 《SENSORS》, 24 May 2020 (2020-05-24), pages 1 - 23 *
张栋等: "互补滤波和卡尔曼滤波的融合姿态解算方法", 《传感器与微系统》, vol. 36, no. 3, 31 March 2017 (2017-03-31), pages 62 - 69 *
朱托: "GPS/SINS组合导航与姿态测量的设计", 《中国优秀硕士学位论文全文数据库（电子期刊）基础科技辑》, no. 3, 15 March 2015 (2015-03-15) *
杜署明: "基于捷联惯性传感的吊钩姿态估计技术研究", 《中国优秀硕士学位论文全文数据库（电子期刊）工程科技II辑》, no. 3, 15 March 2016 (2016-03-15) *

Cited By (24)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113175926B (zh) * 2021-04-21 2022-06-21 哈尔滨工程大学 一种基于运动状态监测的自适应水平姿态测量方法
WO2022222938A1 (zh) * 2021-04-21 2022-10-27 哈尔滨工程大学 一种基于运动状态监测的自适应水平姿态测量方法
CN113175926A (zh) * 2021-04-21 2021-07-27 哈尔滨工程大学 一种基于运动状态监测的自适应水平姿态测量方法
CN113447018A (zh) * 2021-07-06 2021-09-28 北京理工导航控制科技股份有限公司 一种水下惯性导航系统的姿态实时估计方法
CN113701747A (zh) * 2021-07-20 2021-11-26 北京航天控制仪器研究所 一种基于离心机激励的惯性测量系统姿态角误差分离方法
CN113776529B (zh) * 2021-09-13 2024-04-19 中国人民解放军海军工程大学 一种基于载体系四元数姿态误差的非线性误差模型
CN114061571A (zh) * 2021-11-12 2022-02-18 同济大学 一种自适应梯度下降惯性测量单元的姿态解算方法及系统
CN114061571B (zh) * 2021-11-12 2023-08-04 同济大学 一种自适应梯度下降惯性测量单元的姿态解算方法及系统
CN114111770A (zh) * 2021-11-19 2022-03-01 清华大学 一种水平姿态测量方法、系统、处理设备及存储介质
CN114111770B (zh) * 2021-11-19 2024-04-09 清华大学 一种水平姿态测量方法、系统、处理设备及存储介质
CN114111771A (zh) * 2021-11-25 2022-03-01 九江中船仪表有限责任公司（四四一厂） 一种双轴稳定平台的动态姿态测量方法
CN114166216A (zh) * 2021-11-25 2022-03-11 哈尔滨工程大学 一种aru正置与倒置安装的水平姿态测量方法
CN114166216B (zh) * 2021-11-25 2023-07-21 哈尔滨工程大学 一种aru正置与倒置安装的水平姿态测量方法
CN114323007A (zh) * 2021-12-30 2022-04-12 西人马帝言(北京)科技有限公司 一种载体运动状态估计方法及装置
CN114383613A (zh) * 2022-01-18 2022-04-22 广东工业大学 一种基于北斗定向的电力线舞动监测系统和方法
CN114383613B (zh) * 2022-01-18 2023-12-15 广东工业大学 一种基于北斗定向的电力线舞动监测系统和方法
CN114485641A (zh) * 2022-01-24 2022-05-13 武汉梦芯科技有限公司 一种基于惯导卫导方位融合的姿态解算方法及装置
CN114485641B (zh) * 2022-01-24 2024-03-26 武汉梦芯科技有限公司 一种基于惯导卫导方位融合的姿态解算方法及装置
CN115031785A (zh) * 2022-06-21 2022-09-09 浙江大学 一种基于多传感器融合技术的软土勘测方法
CN114911252B (zh) * 2022-07-15 2022-09-30 北京航天驭星科技有限公司 基于遥测数据确定火箭姿态的方法、装置、设备、介质
CN114911252A (zh) * 2022-07-15 2022-08-16 北京航天驭星科技有限公司 基于遥测数据确定火箭姿态的方法、装置、设备、介质
CN116674701A (zh) * 2023-04-26 2023-09-01 上海新纪元机器人有限公司 减摇防晕座椅及其控制方法和系统
CN116588261B (zh) * 2023-07-03 2024-02-09 上海新纪元机器人有限公司 座椅主动补偿控制方法及系统
CN116588261A (zh) * 2023-07-03 2023-08-15 上海新纪元机器人有限公司 座椅主动补偿控制方法及系统

Also Published As

Publication number Publication date
CN112629538B (zh) 2023-02-14

Similar Documents

Publication Publication Date Title
CN112629538B (zh) 基于融合互补滤波和卡尔曼滤波的舰船水平姿态测量方法
CN112504275B (zh) 一种基于级联卡尔曼滤波算法的水面舰船水平姿态测量方法
CN107655476B (zh) 基于多信息融合补偿的行人高精度足部导航方法
CN110501024B (zh) 一种车载ins/激光雷达组合导航系统的量测误差补偿方法
CN109813311B (zh) 一种无人机编队协同导航方法
CN110221332B (zh) 一种车载gnss/ins组合导航的动态杆臂误差估计和补偿方法
CN109459044B (zh) 一种gnss双天线辅助的车载mems惯导组合导航方法
CN107588769B (zh) 一种车载捷联惯导、里程计及高程计组合导航方法
US6459990B1 (en) Self-contained positioning method and system thereof for water and land vehicles
KR101168100B1 (ko) 차량의 위치, 자세 및 헤딩을 추측하는 시스템 및 방법
CN109000640B (zh) 基于离散灰色神经网络模型的车辆gnss/ins组合导航方法
CN111121766B (zh) 一种基于星光矢量的天文与惯性组合导航方法
CN110954102B (zh) 用于机器人定位的磁力计辅助惯性导航系统及方法
CN110017837B (zh) 一种姿态抗磁干扰的组合导航方法
CN112432642B (zh) 一种重力灯塔与惯性导航融合定位方法及系统
CN111722295B (zh) 一种水下捷联式重力测量数据处理方法
CN110849360B (zh) 面向多机协同编队飞行的分布式相对导航方法
CN111141273A (zh) 基于多传感器融合的组合导航方法及系统
CN112880669A (zh) 一种航天器星光折射和单轴旋转调制惯性组合导航方法
CN114964222A (zh) 一种车载imu姿态初始化方法、安装角估计方法及装置
CN108416387B (zh) 基于gps与气压计融合数据的高度滤波方法
CN108871319B (zh) 一种基于地球重力场与地磁场序贯修正的姿态解算方法
Wang et al. State transformation extended Kalman filter for SINS based integrated navigation system
CN113008229A (zh) 一种基于低成本车载传感器的分布式自主组合导航方法
CN111307114B (zh) 基于运动参考单元的水面舰船水平姿态测量方法

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
GR01 Patent grant
GR01 Patent grant