CN113218421A - 北斗拒止条件下捷联惯导***鲁棒自适应动态对准方法 - Google Patents
北斗拒止条件下捷联惯导***鲁棒自适应动态对准方法 Download PDFInfo
- Publication number
- CN113218421A CN113218421A CN202110513665.2A CN202110513665A CN113218421A CN 113218421 A CN113218421 A CN 113218421A CN 202110513665 A CN202110513665 A CN 202110513665A CN 113218421 A CN113218421 A CN 113218421A
- Authority
- CN
- China
- Prior art keywords
- array
- robust
- adaptive
- beidou
- innovation
- 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
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)
Abstract
本发明属于自适应滤波技术领域,公开了一种北斗拒止条件下捷联惯导***鲁棒自适应动态对准方法,初始对准滤波模型的构建;进行卡尔曼滤波时间更新;计算观测量与观测新息之间的马氏距离,进行观测新息权值计算;基于Sage‑Husa滤波方法重新定义R阵自适应估计方程使其具有鲁棒性,并对R阵进行鲁棒自适应估计;通过引入膨胀因子,修正估计出的R阵;利用修正后的R阵进行KF的量测更新。本发明既能够保证R阵的鲁棒自适应性,也能保证整个滤波器的鲁棒性,即能在对R阵进行自适应估计的同时,实现鲁棒滤波。本发明的鲁棒自适应KF算法能够在复杂环境下实现对R阵的实时估计,有效克服观测异常值的不良影响。
Description
技术领域
本发明属于自适应滤波技术领域,尤其涉及一种北斗拒止条件下捷联惯导***鲁棒自适应动态对准方法(SHRAKF)、***、介质、水下航行器。
背景技术
目前,自主式水下航行器(Autonomous Underwater Vehicle,AUV)是海洋资源勘探、开发,海洋工程以及科学研究的主要运载体,其自主导航***主要依赖惯性导航***(Inertial navigation system,INS)。捷联式惯导***(Strapdown inertial navigationsystem,SINS)凭借其自主性强、隐蔽性高、结构简单、体积小及成本低等优点,在水下定位导航授时(Position,Navigation and Timing,PNT)领域受到广泛关注,并成为AUV自主导航定位的重要工具。然而,SINS的本质是基于牛顿第二定律的积分推算***,误差会随着时间而积累。由此可知,任何初始误差如初始姿态误差、惯性测量单元(InertialMeasurement Unit,IMU)器件误差等都会进入到积分***造成积分运算误差,进而影响SINS长航时、长航程的导航精度。因此,准确地确定积分推算***的积分初始值对实现SINS精确导航有着至关重要的作用。初始对准是水下导航的前置步骤,是确保高精度导航的重要前提。而水下环境拒止全球导航卫星***(Global Navigation Satellite System,GNSS),如北斗卫星导航***(简称“北斗”)信号,所以无法利用GNSS进行初始对准,研究不依赖北斗的水下初始对准技术是水下PNT领域中的一大技术重、难点。在无北斗辅助条件下,通常会用多普勒计程仪(Doppler velocity log,DVL)辅助SINS进行水下动态初始对准。
卡尔曼滤波(Kalman Filter,KF)方法是初始对准***中广泛使用的一种信息融合方式,该算法采用预建噪声模型,认为噪声为统计特性已知的不相关高斯白噪声。利用KF处理线性、无偏***问题时,***的干扰与量测噪声的统计特性先验已知,则可以通过滤波获得***状态的最优估计。但是,在水下初始对准中,由于深海环境中情况复杂,如洋流干扰、海底地质环境、海水温度、盐度变化、鱼群影响等均会影响DVL对速度的测量,速度辅助信息容易受到野值等非高斯噪声的污染,这些不确定因素将导致KF中的量测噪声阵不够准确,从而导致滤波性能降低,甚至滤波发散。因此,亟需一种北斗拒止条件下捷联惯导***鲁棒自适应动态对准方法。
通过上述分析,现有技术存在的问题及缺陷为:在水下动态初始对准中,由于深海环境中情况复杂,会影响DVL对速度的测量,外部辅助信息容易受到野值等非高斯噪声的污染,这些不确定因素也将导致KF中的量测噪声阵R阵不够准确,从而使滤波性能降低,甚至发散。
解决以上问题及缺陷的难度为:对于长航时、长航程的AUV而言,其工作的外部环境复杂、多变,并且存在很多无法避免的不可控因素,高精度姿态测量、确定是保证其正常航行的重要前提。不同于陆上或水面上,由于电磁波信号在水下衰减极快,导致水下SINS的动态启动以及长航时、长航程导航无法获取来自北斗的速度、位置信息的辅助。在水面以上,运载体可以通过卫星定位测姿或卫星/INS组合测姿获取准确的姿态信息。而在水下无GNSS信号的环境中,SINS初始姿态的确定尤其是动态条件下初始对准尤为困难。
解决以上问题及缺陷的意义为:对无北斗信号条件下SINS动态初始对准方法的研究,对提高AUV机动性、隐蔽性、长航时长航程和高精度的导航能力具有重要的现实意义。
发明内容
针对现有技术存在的问题,本发明提供了一种北斗拒止条件下捷联惯导***鲁棒自适应动态对准方法(SHRAKF)、***、介质、水下航行器。
本发明是这样实现的,一种北斗拒止条件下捷联惯导***鲁棒自适应动态对准方法(SHRAKF),包括以下步骤:
步骤一,初始对准滤波模型的构建;
步骤二,进行KF算法的时间更新,即进行SHRAKF时间更新;
步骤三,进行观测量与观测新息之间的马氏距离计算,计算观测新息对应的权值;
步骤四,基于Sage Husa方法设计量测噪声阵(R阵)鲁棒自适应估计策略,重新定义R阵鲁棒自适应估计表达式,进行R阵自适应估计;
步骤五,基于马氏距离算法,引入膨胀因子,对估计出的R阵进行修正;
步骤六,利用修正后的R阵进行KF算法量测更新,即进行SHRAKF的量测更新。
进一步,步骤一中,所述考虑初始对准滤波模型的构建,包括:
考虑初始对准滤波模型如式(1)和式(2)所示:
xk=Fk-1xk-1+wk-1 (1)
zk=Hkxk+υk (2)
式中,xk、wk-1分别为状态向量和过程噪声向量;wk~N(0,Qk),Qk为过程噪声协方差阵;zk、υk分别为观测量和量测噪声向量,且υk通常假定服从正态分布,即υk~N(0,Rk),Rk为量测噪声协方差阵。
进一步,步骤二中,所述进行KF的时间更新,即进行SHRAKF时间更新,包括:
k时刻SHRAKF时间更新方程如下:
进一步,步骤三中,所述进行观测量与观测新息之间的马氏距离计算,观测新息对应的权值计算,包括:
进一步,步骤四中,所述基于Sage Husa方法设计量测噪声阵(R阵)鲁棒自适应估计策略,重新定义R阵鲁棒自适应估计表达式,包括:
Pk=(I-KkHk)Pk|k-1 (10)
在高斯条件下,基于利用Sage Husa方法,k时刻R阵自适应估计的一般表达式如下:
式中,0.9≤b<1为衰减因子,取经验值;式(11)对异常观测量是没有鲁棒性的,若k时刻观测量异常,即μk异常,估计出来的也是不准确的。结合式(7)和式(11),重新定义非高斯条件下k时刻R阵自适应估计表达式如下:
将式(13)带入式(6)可得:
式(14)可以转化为求解κk的非线性问题,如式(15)所示:
式中,κk可以通过牛顿迭代法求解;κk(i+1)与κk(i)的关系可表示为:
进一步,步骤六中,所述利用修正后的R阵进行KF量测更新,即进行SHRAKF量测更新,包括:
SHRAKF量测更新过程如下:
Pk=(I-KkHk)Pk|k-1 (21)。
本发明的另一目的在于提供一种应用所述的北斗拒止条件下捷联惯导***鲁棒自适应动态对准方法的北斗拒止条件下捷联惯导***鲁棒自适应动态对准***,所述北斗拒止条件下捷联惯导***鲁棒自适应动态对准***包括:
对准滤波模型构建模块,用于考虑初始对准滤波模型的构建;
时间更新模块,用于进行KF的时间更新,即进行北斗拒止条件下捷联惯导***鲁棒自适应动态对准方法(SHRAKF)时间更新;
计算观测新息权值模块,用于进行观测量与观测新息之间马氏距离计算以及进行观测新息权值计算;
量测噪声阵鲁棒自适应估计模块,用于在非高斯条件下进行量测噪声阵鲁棒自适应估计;
修正模块,用于通过马氏距离算法引入膨胀因子,修正估计出的量测噪声协方差阵;
量测更新模块,用于进行SHRAKF的量测更新。
本发明的另一目的在于提供一种自主式水下航行器,所述自主式水下航行器执行所述的北斗拒止条件下捷联惯导***鲁棒自适应动态对准方法。
本发明的另一目的在于提供一种计算机可读存储介质,储存有指令,当所述指令在计算机上运行时,使得计算机执行所述的北斗拒止条件下捷联惯导***鲁棒自适应动态对准方法。
结合上述的所有技术方案,本发明所具备的优点及积极效果为:
本发明通过动态初始对准滤波模型的构建;进行卡尔曼滤波(KF)时间更新;计算观测量与观测新息之间的马氏距离,进行观测新息权值计算;基于Sage-Husa滤波方法重新定义R阵自适应估计方程使其具有鲁棒性,并对R阵进行鲁棒自适应估计;通过引入膨胀因子,修正估计出的R阵;利用修正后的R阵进行KF的量测更新,即为SHRAKF的量测更新过程。本发明既能够保证R阵的鲁棒自适应性,也能保证整个滤波器的鲁棒性,即SHRAKF能在对R阵进行自适应估计的同时,实现鲁棒滤波。本发明的鲁棒自适应KF算法能够在复杂环境下实现对R阵的实时估计,有效克服观测异常值的不良影响。
相比于现有技术,本发明的优点进一步包括:
本发明提供的北斗拒止条件下捷联惯导***鲁棒自适应动态对准方法(SHRAKF),既能够在北斗拒止的水下复杂环境下保证量测噪声阵的鲁棒自适应性,也保证了整个滤波器的鲁棒性。也就是说,SHRAKF能在对量测噪声阵进行自适应估计的同时,实现鲁棒滤波。本发明的SHRAKF具有以下特点:1.能够在复杂环境下实现对R阵的实时估计(实时性);2.能够有效克服观测异常值的不良影响(鲁棒性)。
选取900s船载实测数据进行初始对准试验。其中,惯性测量单元(InertialMeasurement Unit,IMU)和多普勒计程仪(DVL)性能指标分别如表1,表2所示。DVL输出速度如图3所示。
表1本实施例中IMU性能指标
主要技术指标 | 陀螺仪 | 加速度计 |
测量范围 | ±200°/s | ±15g |
更新频率 | 200Hz | 200Hz |
精度等级 | <0.02°/h(1σ) | <50ug(1σ) |
标度因数重复性 | ≤30ppm | ≤50ppm |
表2本实施例中DVL性能指标
主要技术指标 | |
测速精度 | 0.5%V±0.5cm/s |
测速范围 | -10knot~20knot |
更新频率 | 1Hz |
发射频率 | 300kHz |
底跟踪深度 | 300m |
试验中,设置SINS初始失准角为[1°;1°;1°],式(7)中统计门限η2设置为在实际应用中,量测噪声统计特性先验信息通常是不确定或未知的,因此假设初始量测噪声阵R0是不准确的。试验中,设置R0=diag([0.012,0.012])m2/s2。
对于水下SINS动态初始对准,外部复杂环境是不可避免的,也就是说,DVL输出速度不可避免地会受到非高斯噪声的污染。本发明提供的北斗拒止条件下捷联惯导***鲁棒自适应动态对准方法简记为SHRAKF,传统的卡尔曼滤波方法简记为KF,基于Sage Husa算法的自适应KF方法简记为SHAKF。为进一步验证本发明提供的SHRAKF在非高斯条件下的有效性、可行性和优势,分别利用KF、SHAKF、SHRAKF进行初始对准试验,姿态、速度和位置对准结果如图4-图6所示。
图4-图6中,黑色虚线、浅灰虚线和实线分别表示由KF、SHAKF和SHRAKF得到的初始对准误差曲线。由图4-图6可以明显看出,在观测量受到野值污染时刻,KF得到的初始对准误差曲线出现跳变。由图4和图5可以看出,SHAKF得到的水平姿态角和速度对准误差曲线相比于KF具备较好的收敛性。然而,SHAKF得到的航向角及位置对准误差曲线在受野值污染时刻发生跳变。SHRAKF得到的初始对准误差曲线在整个动态对准过程中是稳定、收敛的。相比于KF,SHAKF,SHRAKF具备更强鲁棒性和更高对准精度。
图7表示分别利用SHAKF和SHRAKF得到的R阵分量自适应估计结果。可以明显看出,相比于SHAKF,利用SHRAKF得到的R阵分量自适应估计曲线更加稳定。实际的R阵约为diag(0.01,0.01)m2/s2。经计算,SHRAKF方法对R(1,1)估计值的时间平均(0~900s)为0.0074m2/s2,对R(2,2)估计值的时间平均(0~900s)为0.0100m2/s2;SHAKF方法对R(1,1)估计值的时间平均(0~900s)为1.5611m2/s2,对R(2,2)估计值的时间平均(0~900s)为11.1005m2/s2。因此,在非高斯条件下,相比于SHAKF,SHRAKF对R阵具有更高的自适应估计精度及稳定性。
根据试验结果,不同方法得到的航向角和位置对准误差标准差如表3所示。
表3不同方法得到初始对准误差的标准差
航向角(°) | 位置(m) | |
KF | 3.4970 | 130.0000 |
SHAKF | 0.5721 | 16.7800 |
SHRAKF | 0.3602 | 6.8680 |
由表3可知,SHRAKF得到的对准误差标准差小于KF和SHAKF。也就是说,相比于KF和SHAKF,SHRAKF得到的初始对准误差更加稳定。初始对准试验结果验证了SHRAKF的鲁棒性和自适应性。
附图说明
为了更清楚地说明本发明实施例的技术方案,下面将对本发明实施例中所需要使用的附图做简单的介绍,显而易见地,下面所描述的附图仅仅是本发明的一些实施例,对于本领域普通技术人员来讲,在不付出创造性劳动的前提下还可以根据这些附图获得其他的附图。
图1是本发明实施例提供的北斗拒止条件下捷联惯导***鲁棒自适应动态对准方法流程图。
图2是本发明实施例提供的北斗拒止条件下捷联惯导***鲁棒自适应动态对准***结构框图;
图中:1、对准滤波模型构建模块;2、时间更新模块;3、计算观测新息权值模块;4、量测噪声阵(R阵)鲁棒自适应估计模块;5、修正模块;6、量测更新模块。
图3是本发明实施例提供的选取900s船载实测数据进行初始对准试验中DVL输出速度图。
图4是本发明实施例提供的不同方法姿态角对准误差示意图。其中,图4(a)俯仰角对准误差图;图4(b)横滚角对准误差图;图4(c)横滚角对准误差图。
图5是本发明实施例提供的不同方法速度对准误差示意图。其中,图5(a)东向速度对准误差图;图5(b)北向速度对准误差图。
图6是本发明实施例提供的不同方法位置对准误差图。
图7是本发明实施例提供的不同方法量测噪声协方差分量自适应估计示意图。
具体实施方式
为了使本发明的目的、技术方案及优点更加清楚明白,以下结合实施例,对本发明进行进一步详细说明。应当理解,此处所描述的具体实施例仅仅用以解释本发明,并不用于限定本发明。
针对现有技术存在的问题,本发明提供了一种北斗拒止条件下捷联惯导***鲁棒自适应动态对准方法(SHRAKF)及***,下面结合附图对本发明作详细的描述。
如图1所示,本发明实施例提供的北斗拒止条件下捷联惯导***鲁棒自适应动态对准方法及***包括以下步骤:
S101,考虑初始对准滤波模型的构建;
S102,进行KF算法的时间更新,即进行SHRAKF时间更新;
S103,进行观测量与观测新息之间的马氏距离计算,计算观测新息对应的权值;
S104,基于Sage Husa方法设计量测噪声阵(R阵)鲁棒自适应估计策略,重新定义R阵鲁棒自适应估计表达式,进行R阵自适应估计;
S105,基于马氏距离算法,引入膨胀因子,对估计出的R阵进行修正;
S106,利用修正后的R阵进行KF算法量测更新,即进行基于Sage Husa方法的鲁棒自适应KF算法SHRAKF的量测更新。
如图2所示,本发明实施例提供的北斗拒止条件下捷联惯导***鲁棒自适应动态对准***,包括:
对准滤波模型构建模块1,用于考虑初始对准滤波模型的构建;
时间更新模块2,用于进行KF的时间更新,即进行SHRAKF的时间更新;
计算观测新息权值模块3,用于进行观测量与观测新息之间马氏距离计算以及进行观测新息权值计算;
量测噪声阵(R阵)鲁棒自适应估计模块4,用于在非高斯条件下进行R阵鲁棒自适应估计;
修正模块5,用于通过马氏距离算法引入膨胀因子,修正估计出的量测噪声协方差阵;
量测更新模块6,用于进行SHRAKF的量测更新。
下面结合实施例对本发明作进一步描述。
实施例:实验验证与结果分析
1、数据来源
试验数据是从一套船载实验***中采集得到的,其中,惯性测量单元(InertialMeasurement Unit,IMU)和多普勒计程仪(DVL)性能指标分别如表1,表2所示。DVL输出速度如图3所示。
表1本实施例中IMU性能指标
主要技术指标 | 陀螺仪 | 加速度计 |
测量范围 | ±200°/s | ±15g |
更新频率 | 200Hz | 200Hz |
精度等级 | <0.02°/h(1σ) | <50ug(1σ) |
标度因数重复性 | ≤30ppm | ≤50ppm |
表2本实施例中DVL性能指标
2、实验验证与结果分析
选取900s船载实测数据进行初始对准试验。试验中,设置SINS初始失准角为[1°;1°;1°],式(7)中统计门限η2设置为在实际应用中,量测噪声统计特性先验信息通常是不确定或未知的,因此假设初始量测噪声阵是不准确的,即R0=diag([0.012,0.012])m2/s2。
对于水下SINS动态初始对准,外部复杂环境是不可避免的,也就是说,DVL输出速度不可避免地会受到非高斯噪声的污染。本发明提供的新的鲁棒自适应KF方法简记为SHRAKF,传统的卡尔曼滤波方法简记为KF,基于Sage Husa算法的自适应卡尔曼滤波方法简记为SHAKF。为进一步验证本发明提供的SHRAKF在非高斯条件下的有效性、可行性和优势,分别利用KF、SHAKF、SHRAKF进行初始对准试验,姿态、速度和位置对准结果如图4-图6所示。
图4-图6中,黑色虚线、浅灰虚线和红色实线分别表示由KF、SHAKF和SHRAKF得到的初始对准误差曲线。由图4-图6可以明显看出,在观测量受到野值污染时刻,KF得到的初始对准误差曲线出现跳变。由图4和图5可以看出,SHAKF得到的水平姿态角和速度对准误差曲线相比于KF具备较好的收敛性。然而,SHAKF得到的航向角及位置对准误差曲线在受野值污染时刻发生跳变。SHRAKF得到的初始对准误差曲线在整个动态对准过程中是稳定、收敛的。相比于KF,SHAKF,SHRAKF具备更强鲁棒性和更高对准精度。
图7表示分别利用SHAKF和SHRAKF得到的R阵分量自适应估计结果。可以明显看出,相比于SHAKF,利用SHRAKF得到的R阵分量自适应估计曲线更加稳定。实际的R阵约为diag(0.01,0.01)m2/s2。经计算,SHRAKF方法对R(1,1)估计值的时间平均(0~900s)为0.0074m2/s2,对R(2,2)估计值的时间平均(0~900s)为0.0100m2/s2;SHAKF方法对R(1,1)估计值的时间平均(0~900s)为1.5611m2/s2,对R(2,2)估计值的时间平均(0~900s)为11.1005m2/s2。因此,在非高斯条件下,相比于SHAKF,SHRAKF对R阵具有更高的自适应估计精度及稳定性。
根据试验结果,不同方法得到的航向角和位置对准误差标准差如表3所示。
表3不同方法得到初始对准误差的标准差
航向角(°) | 位置(m) | |
KF | 3.4970 | 130.0000 |
SHAKF | 0.5721 | 16.7800 |
SHRAKF | 0.3602 | 6.8680 |
由表3可知,SHRAKF得到的对准误差标准差小于KF和SHAKF。也就是说,相比于KF和SHAKF,SHRAKF得到的初始对准误差更加稳定。初始对准试验结果验证了SHRAKF的鲁棒性和自适应性。
在上述实施例中,可以全部或部分地通过软件、硬件、固件或者其任意组合来实现。当使用全部或部分地以计算机程序产品的形式实现,所述计算机程序产品包括一个或多个计算机指令。在计算机上加载或执行所述计算机程序指令时,全部或部分地产生按照本发明实施例所述的流程或功能。所述计算机可以是通用计算机、专用计算机、计算机网络、或者其他可编程装置。所述计算机指令可以存储在计算机可读存储介质中,或者从一个计算机可读存储介质向另一个计算机可读存储介质传输,例如,所述计算机指令可以从一个网站站点、计算机、服务器或数据中心通过有线(例如同轴电缆、光纤、数字用户线(DSL)或无线(例如红外、无线、微波等)方式向另一个网站站点、计算机、服务器或数据中心进行传输)。所述计算机可读取存储介质可以是计算机能够存取的任何可用介质或者是包含一个或多个可用介质集成的服务器、数据中心等数据存储设备。所述可用介质可以是磁性介质,(例如,软盘、硬盘、磁带)、光介质(例如,DVD)、或者半导体介质(例如固态硬盘SolidState Disk(SSD))等。
以上所述,仅为本发明的具体实施方式,但本发明的保护范围并不局限于此,任何熟悉本技术领域的技术人员在本发明揭露的技术范围内,凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等,都应涵盖在本发明的保护范围之内。
Claims (10)
1.一种北斗拒止条件下捷联惯导***鲁棒自适应动态对准方法,其特征在于,所述北斗拒止条件下捷联惯导***鲁棒自适应动态对准方法包括以下步骤:
步骤一,初始对准滤波模型的构建;
步骤二,进行KF算法的时间更新;
步骤三,进行观测量与观测新息之间的马氏距离计算,计算观测新息对应的权值;
步骤四,基于Sage Husa方法设计量测噪声阵(R阵)鲁棒自适应估计策略,重新定义R阵鲁棒自适应估计表达式,进行R阵自适应估计;
步骤五,基于马氏距离算法,引入膨胀因子,对估计出的R阵进行修正;
步骤六,利用修正后的R阵进行KF算法量测更新,即进行基于Sage Husa方法的鲁棒自适应KF算法SHRAKF的量测更新。
2.如权利要求1所述的北斗拒止条件下捷联惯导***鲁棒自适应动态对准方法,其特征在于,所述步骤一初始对准滤波模型的构建,包括:
考虑初始对准滤波模型如式(1)和式(2)所示:
xk=Fk-1xk-1+wk-1 (1)
zk=Hkxk+υk (2)
式中,xk、wk-1分别为状态向量和过程噪声向量;wk~N(0,Qk),Qk为过程噪声协方差阵;zk、υk分别为观测量和量测噪声向量,且υk通常假定服从正态分布,即υk~N(0,Rk),Rk为量测噪声协方差阵。
5.如权利要求1所述的北斗拒止条件下捷联惯导***鲁棒自适应动态对准方法,其特征在于,所述步骤四基于Sage Husa方法设计量测噪声阵(R阵)鲁棒自适应估计策略,重新定义R阵鲁棒自适应估计表达式,包括:
Pk=(I-KkHk)Pk|k-1 (10)
在高斯条件下,基于利用Sage Husa方法,k时刻R阵自适应估计的一般表达式如下:
式中,0.9≤b<1为衰减因子,取经验值;式(11)对异常的观测量没有鲁棒性,若k时刻的观测量异常,即μk异常,估计出来的不准确;结合式(7)和式(11),重新定义非高斯条件下k时刻R阵自适应估计表达式如下:
8.一种北斗拒止条件下捷联惯导***鲁棒自适应动态对准***,其特征在于,所述北斗拒止条件下捷联惯导***鲁棒自适应动态对准***包括:
对准滤波模型构建模块,用于考虑初始对准滤波模型的构建;
时间更新模块,用于进行KF的时间更新,即进行SHRAKF时间更新;
计算观测新息权值模块,用于进行观测量与观测新息之间马氏距离计算以及进行观测新息权值计算;
量测噪声阵鲁棒自适应估计模块,用于在非高斯条件下进行量测噪声阵鲁棒自适应估计;
修正模块,用于通过马氏距离算法引入膨胀因子,修正估计出的量测噪声协方差阵;
量测更新模块,用于进行SHRAKF的量测更新。
9.一种计算机可读存储介质,储存有指令,当所述指令在计算机上运行时,使得计算机执行如权利要求1~7任意一项所述的北斗拒止条件下捷联惯导***鲁棒自适应动态对准方法。
10.一种自主式水下航行器,其特征在于,所述自主式水下航行器执行如权利要求1~7任意一项所述的北斗拒止条件下捷联惯导***鲁棒自适应动态对准方法。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202110513665.2A CN113218421B (zh) | 2021-05-11 | 2021-05-11 | 北斗拒止条件下捷联惯导***鲁棒自适应动态对准方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202110513665.2A CN113218421B (zh) | 2021-05-11 | 2021-05-11 | 北斗拒止条件下捷联惯导***鲁棒自适应动态对准方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN113218421A true CN113218421A (zh) | 2021-08-06 |
CN113218421B CN113218421B (zh) | 2023-07-04 |
Family
ID=77095055
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202110513665.2A Active CN113218421B (zh) | 2021-05-11 | 2021-05-11 | 北斗拒止条件下捷联惯导***鲁棒自适应动态对准方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN113218421B (zh) |
Cited By (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN114777812A (zh) * | 2022-04-17 | 2022-07-22 | 中国人民解放军国防科技大学 | 一种水下组合导航***行进间对准与姿态估计方法 |
CN115096321A (zh) * | 2022-06-23 | 2022-09-23 | 中国人民解放军63921部队 | 一种车载捷联惯导***鲁棒无迹信息滤波对准方法及*** |
Citations (4)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN108759870A (zh) * | 2018-07-03 | 2018-11-06 | 哈尔滨工业大学 | 一种基于新型鲁棒广义高阶容积卡尔曼滤波的传递对准方法 |
CN109443379A (zh) * | 2018-09-28 | 2019-03-08 | 东南大学 | 一种深海潜航器的sins/dvl水下抗晃动对准方法 |
CN109974695A (zh) * | 2019-04-09 | 2019-07-05 | 东南大学 | 基于Krein空间的水面舰艇导航***的鲁棒自适应滤波方法 |
US20210080287A1 (en) * | 2019-09-18 | 2021-03-18 | Harbin Engineering University | Method for initial alignment of radar assisted airborne strapdown inertial navigation system |
-
2021
- 2021-05-11 CN CN202110513665.2A patent/CN113218421B/zh active Active
Patent Citations (4)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN108759870A (zh) * | 2018-07-03 | 2018-11-06 | 哈尔滨工业大学 | 一种基于新型鲁棒广义高阶容积卡尔曼滤波的传递对准方法 |
CN109443379A (zh) * | 2018-09-28 | 2019-03-08 | 东南大学 | 一种深海潜航器的sins/dvl水下抗晃动对准方法 |
CN109974695A (zh) * | 2019-04-09 | 2019-07-05 | 东南大学 | 基于Krein空间的水面舰艇导航***的鲁棒自适应滤波方法 |
US20210080287A1 (en) * | 2019-09-18 | 2021-03-18 | Harbin Engineering University | Method for initial alignment of radar assisted airborne strapdown inertial navigation system |
Cited By (3)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN114777812A (zh) * | 2022-04-17 | 2022-07-22 | 中国人民解放军国防科技大学 | 一种水下组合导航***行进间对准与姿态估计方法 |
CN114777812B (zh) * | 2022-04-17 | 2024-04-05 | 中国人民解放军国防科技大学 | 一种水下组合导航***行进间对准与姿态估计方法 |
CN115096321A (zh) * | 2022-06-23 | 2022-09-23 | 中国人民解放军63921部队 | 一种车载捷联惯导***鲁棒无迹信息滤波对准方法及*** |
Also Published As
Publication number | Publication date |
---|---|
CN113218421B (zh) | 2023-07-04 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN109283562B (zh) | 一种车联网中车辆三维定位方法及装置 | |
WO2022088797A1 (zh) | 考虑量测异常的集群式多深海潜航器的协同定位方法 | |
CN110207695B (zh) | 一种适用于深海auv的无速度辅助单信标定位方法 | |
CN113218421A (zh) | 北斗拒止条件下捷联惯导***鲁棒自适应动态对准方法 | |
CN111596333B (zh) | 一种水下定位导航方法及*** | |
CN110132281B (zh) | 一种基于询问应答模式的水下高速目标高精度自主声学导航方法 | |
CN111174774A (zh) | 一种某深度水层面模式下的导航信息融合方法及*** | |
CN115096302A (zh) | 捷联惯性基导航***信息滤波鲁棒对准方法、***及终端 | |
Das et al. | An experimental study on relative and absolute pose graph fusion for vehicle localization | |
CN110703205A (zh) | 基于自适应无迹卡尔曼滤波的超短基线定位方法 | |
Wang et al. | A novel adaptive sliding observation-based cooperative positioning algorithm under factor graph framework for multiple UUVs | |
CN111708008B (zh) | 一种基于imu和tof的水下机器人单信标导航方法 | |
CN110763234B (zh) | 一种水下机器人海底地形匹配导航路径规划方法 | |
CN116026324B (zh) | 用于水-空跨介质航行器的跨域导航***和方法 | |
Wu et al. | Improved adaptive iterated extended Kalman filter for GNSS/INS/UWB-integrated fixed-point positioning | |
CN116358544A (zh) | 基于声学特征匹配定位修正惯性导航误差的方法和*** | |
CN113503891B (zh) | 一种sinsdvl对准校正方法、***、介质及设备 | |
CN113821766B (zh) | 一种海洋声学导航***误差改正方法 | |
CN115685165A (zh) | 一种立体阵超短基线定位***安装偏差校准方法及装置 | |
CN113008235B (zh) | 基于矩阵k-l散度的多源导航信息融合方法 | |
CN114577211A (zh) | 考虑洋流影响的基于因子图的主从式auv协同定位方法 | |
Zhang et al. | Calibration method of DVL based on position observation information | |
Yang et al. | Robust sequential adaptive Kalman filter algorithm for ultrashort baseline underwater acoustic positioning | |
CN117848351B (zh) | 基于可信度和统计相似度量测的水下自主导航***及方法 | |
CN113009468B (zh) | 一种视线坐标系内的解耦cmkf跟踪方法及*** |
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 |