CN113819906B - 一种基于统计相似度量的组合导航鲁棒滤波方法 - Google Patents
一种基于统计相似度量的组合导航鲁棒滤波方法 Download PDFInfo
- Publication number
- CN113819906B CN113819906B CN202111115235.1A CN202111115235A CN113819906B CN 113819906 B CN113819906 B CN 113819906B CN 202111115235 A CN202111115235 A CN 202111115235A CN 113819906 B CN113819906 B CN 113819906B
- Authority
- CN
- China
- Prior art keywords
- measurement
- error
- dvl
- vector
- sins
- 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.)
- Active
Links
- 238000005259 measurement Methods 0.000 title claims abstract description 109
- 238000000034 method Methods 0.000 title claims abstract description 32
- 238000001914 filtration Methods 0.000 title claims abstract description 14
- 239000013598 vector Substances 0.000 claims description 51
- 239000011159 matrix material Substances 0.000 claims description 27
- 238000011524 similarity measure Methods 0.000 claims description 9
- 238000012937 correction Methods 0.000 claims description 6
- 238000004364 calculation method Methods 0.000 claims description 5
- 230000009466 transformation Effects 0.000 claims description 5
- 230000005484 gravity Effects 0.000 claims description 3
- 238000000354 decomposition reaction Methods 0.000 abstract description 4
- 230000010365 information processing Effects 0.000 abstract description 3
- 230000026676 system process Effects 0.000 abstract description 3
- 230000008569 process Effects 0.000 description 6
- 238000002474 experimental method Methods 0.000 description 4
- 238000012360 testing method Methods 0.000 description 3
- 238000010586 diagram Methods 0.000 description 2
- 238000012986 modification Methods 0.000 description 2
- 230000004048 modification Effects 0.000 description 2
- 238000007500 overflow downdraw method Methods 0.000 description 2
- 238000002202 sandwich sublimation Methods 0.000 description 2
- 230000002159 abnormal effect Effects 0.000 description 1
- 239000011358 absorbing material Substances 0.000 description 1
- 238000003491 array Methods 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 230000003247 decreasing effect Effects 0.000 description 1
- 230000006872 improvement Effects 0.000 description 1
- 238000009434 installation Methods 0.000 description 1
- 238000012546 transfer Methods 0.000 description 1
- XLYOFNOQVPJJNP-UHFFFAOYSA-N water Substances O XLYOFNOQVPJJNP-UHFFFAOYSA-N 0.000 description 1
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/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
- 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
-
- Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
- Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
- Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
- Y02T90/00—Enabling technologies or technologies with a potential or indirect contribution to GHG emissions mitigation
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Automation & Control Theory (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Navigation (AREA)
Abstract
本发明属于非理想条件下的组合导航技术领域,具体涉及一种基于统计相似度量的组合导航鲁棒滤波方法。本发明考虑SINS/DVL紧组合系统中正常的DVL波束测量信息和大误差的波束测量信息同时出现的情形,针对目前现有的组合导航系统鲁棒滤波器对量测信息处理粗糙,导致正常量测信息易丢失的问题,基于多维量测方程分解和统计相似度量提出了新的鲁棒滤波方法。本发明在将SINS/DVL紧组合导航系统的多维量测方程进行分解的同时,引入统计相似度量(SSM)理论,协助每个波束的测量噪声方差在大测量误差出现时完成各自的自适应更新,最终保证每个波束测量信息处理的独立性。本发明可用于非理想条件下的水下航行器组合导航领域。
Description
技术领域
本发明属于非理想条件下的组合导航技术领域,具体涉及一种基于统计相似度量的组合导航鲁棒滤波方法。
背景技术
捷联惯导/多普勒测速计程仪(SINS/DVL)组合导航系统是目前水下航行器最常用的组合导航方法之一。其中紧组合方式可以利用DVL的原始波束测量信息,容错性好,逐渐被广泛使用。DVL采用多普勒频移原理,通过计算向水底发射的声波频率和接受的反射声波频率之差可以测量载体沿声波波束方向的速度。但是在一些复杂情况下,DVL会出现较大的测量误差,例如,DVL发射的声波波束受海洋生物遮挡无法到达海底,或者当水下航行器驶过海沟时,航行器与海床之间的距离超出DVL声波波束范围,亦或者海底存在强吸波材料(如淤泥等),发出的声波波束不能反射回来。理论上,当DVL的某一个波束出现大的测量误差时,紧组合系统的滤波算法需要能够自动降低对该波束错误信息的利用率。但是目前组合导航系统所使用的鲁棒滤波器多是对所有量测信息的利用率进行统一的调节,对于SINS/DVL紧组合导航系统这种典型的多维量测系统来说,降低对单个大测量误差的波束信息的利用率可能会导致其他正常的波束测量信息在系统中没有被充分使用,从而丢失有用信息。
因此,需要发明在不影响导航系统对正常波束信息利用的情况下降低含有大测量误差的波束信息影响的鲁棒滤波方法。
发明内容
本发明的目的在于提供一种基于统计相似度量的组合导航鲁棒滤波方法。
本发明的目的通过如下技术方案来实现:包括以下步骤:
步骤1:定义坐标系;
定义载体坐标系采用b表示,地心地固坐标系用e表示,“东-北-天”地理坐标系作为导航系采用n表示,惯性坐标系用i表示,DVL框架坐标系用d表示;
定义不同坐标系下的速度:
其中,表示捷联惯导系统在b系中的速度矢量;/>表示n系内捷联惯导系统的速度矢量;/>表示捷联惯导系统在d系中的速度矢量;/>表示d系中DVL波束的速度矢量;
步骤2:建立SINS/DVL紧组合导航系统状态方程;
选取SINS的误差作为状态,包括失准角φ、速度误差δVn、位置误差δp、陀螺常值漂移εb和加速度计常值零偏状态方程如下:
其中,εb表示陀螺常值漂移矢量;是陀螺随机漂移矢量;/>是加速度计常值零偏矢量;/>是加速度计随机零偏矢量;φ表示由俯仰、横滚和偏航失准角组成的失准角矢量;是方向余弦矩阵,表示从b系到n系的变换;/>表示b系下b系相对于i系的角速率矢量,对应的计算误差为/> n=[vE vN vh]T是n系中的载体相对于地面的速度矢量,对应的误差矢量是δvn;fb是加速度计的输出;δgn是重力误差向量在n系中的投影;δp表示由纬度误差δL、经度误差δλ和高度误差δh组成的位置误差矢量;RM和RN分别表示子午线圆和本初垂直圆的曲率半径;
步骤3:建立SINS/DVL紧组合导航系统量测方程,并分解;
量测方程:
Z=HX+v
其中,表示测量噪声;I3×3是三维单位矩阵;[·×]表示向量的叉乘运算;α是波束和载体之间的水平角;/>设置为0°或45°;
DVL各个波束的测量噪声通常情况下是不相关的,其量测噪声协方差阵R=E[vvT]是一个对角阵,因此将量测方程进行等效分解:
其中,Hj表示量测矩阵的第j行;vj是相应的测量噪声,不同的DVL波束的测量噪声不相关,这里是将DVL的四个波束测量值假设为由不同的传感器分别进行测量获得;
步骤4:基于统计相似度量构建状态估计的代价函数;
其中,和/>分别表示状态一步预测结果和一步预测协方差阵;N(X;Xk+1,Pk+1)表示关于X的高斯分布,均值是Xk+1,方差是Pk+1;相似度函数fx(t)选择为fx(t)=-0.5t;相似度函数fz(t)选为/>ω是自由度参数,m是量测维数,m=1;q=4;
步骤5:应用高斯-牛顿迭代求取代价函数的最优值;
步骤5.1:初始化迭代次数t=0,设定迭代初值为
步骤5.2:计算滤波增益
其中, 和/>相当于用来调节一步预测协方差阵和量测噪声方差的辅助参数;
步骤5.3:计算后验状态
步骤5.4:计算估计误差协方差阵
步骤5.5:判断是否满足若不满足,则令t=t+1,返回步骤5.2;否则,令/>输出SINS的误差X;
步骤6:将估计的SINS的误差X反馈给SINS,进行修正;
根据估计的SINS的误差X,获取失准角φ、速度误差δVn、位置误差δp;对于SINS解算出的姿态阵速度/>位置/>姿态误差的反馈修正方法为:
速度误差δVn和位置误差δp直接在SINS的输出中直接减去即可:
本发明的有益效果在于:
本发明考虑SINS/DVL紧组合系统中正常的DVL波束测量信息和大误差的波束测量信息同时出现的情形,针对目前现有的组合导航系统鲁棒滤波器对量测信息处理粗糙,导致正常量测信息易丢失的问题,基于多维量测方程分解和统计相似度量提出了新的鲁棒滤波方法。本发明在将SINS/DVL紧组合导航系统的多维量测方程进行分解的同时,引入统计相似度量(SSM)理论,协助每个波束的测量噪声方差在大测量误差出现时完成各自的自适应更新,最终保证每个波束测量信息处理的独立性。本发明可用于非理想条件下的水下航行器组合导航领域。
附图说明
图1为SINS/DVL紧组合导航系统结构框图。
图2为不同坐标系下速度关系图。
图3为湖试实验轨迹图。
图4为速度误差和定位误差比较图。
图5为DVL波束测量值与辅助参数变化对比图。
图6为本发明的实施流程图。
具体实施方式
下面结合附图对本发明做进一步描述。
本发明的目的在于,针对SINS/DVL紧组合导航系统,提供一种可以对每个DVL波束的测量噪声方差进行单独调节的鲁棒滤波方法,即当某一量测出现较大误差时可以自动增大滤波器中所对应的测量噪声方差阵,而其他正常量测的测量噪声方差阵几乎不进行调节。
步骤一:建立SINS/DVL紧组合导航系统模型;
首先定义常用坐标系,载体坐标系采用b表示,地心地固坐标系用e表示,“东-北-天”地理坐标系作为导航系采用n表示,惯性坐标系用i表示,DVL框架坐标系用d表示。
图1描述了SINS/DVL紧组合导航系统的结构。SINS作为一种主要的导航设备,提供速度、姿态和位置信息。作为辅助导航设备,DVL提供四维测量矢量,即DVL在四个波束方向上相对于海底的速度。将SINS解转化为d坐标系下的速度与DVL波束速度的差值作为导航滤波器的观测量。然后对状态进行估计并反馈给SINS,以减小SINS的导航误差。
在本发明中,假设SINS与DVL之间的安装角误差和DVL的标度因数在设备安装完成后已经进行了标定和补偿。残余的安装角误差和标度因数误差可以忽略不计。因此,本发明只选取SINS的误差作为状态,包括失准角φ、速度误差δVn、位置误差δp、陀螺常值漂移εb和加速度计常值零偏状态方程如下:
式中εb表示陀螺常值漂移矢量,是陀螺随机漂移矢量。是加速度计常值零偏矢量/> 是加速度计随机零偏矢量。φ表示由俯仰、横滚和偏航失准角组成的失准角矢量。是方向余弦矩阵,表示从b系到n系的变换。/>表示b系下b系相对于i系的角速率矢量,对应的计算误差为/>vn=[vE vN vh]T是n系中的载体相对于地面的速度矢量,对应的误差矢量是δvn。fb是加速度计的输出。δgn是重力误差向量在n系中的投影。δp表示由纬度误差δL、经度误差δλ和高度误差δh组成的位置误差矢量。RM和RN分别表示子午线圆和本初垂直圆的曲率半径。
本发明在引入测量方程之前,定义了不同坐标系下的速度,以四波束配置DVL为例,如图2所示。是捷联惯导系统在b系中的速度矢量。/>表示n系内捷联惯导系统的速度矢量。/>表示捷联惯导系统在d系中的速度矢量。/>表示d系中DVL波束的速度矢量。
捷联惯导系统在导航系下的速度与d系下的DVL波束速度之间的关系如下
其中α是波束和载体之间的水平角。通常,设置为0°或45°。
DVL波束测量值建模如下:
其中表示DVL的测量噪声。将捷联惯导系统的速度输出转换到d系的结果为
式中,I3×3是三维单位矩阵。[·×]表示向量的叉乘运算。
联立(19)和(20)可以得到量测方程
Z=HX+v (22)
表示测量噪声。
步骤二:分解SINS/DVL紧组合导航系统的多维量测方程;
DVL各个波束的测量噪声通常情况下是不相关的,其量测噪声协方差阵R=E[vvT]是一个对角阵。因此在本发明中可以将量测方程(22)进行等效分解。
其中Hj表示量测矩阵的第j行。vj是相应的测量噪声,不同的DVL波束的测量噪声不相关。这里是将DVL的四个波束测量值假设为由不同的传感器分别进行测量获得。
步骤三:引入统计相似度量概念,并基于统计相似度量构建状态估计的代价函数;
步骤3.1:引入统计相似度量(SSM)
统计相似度度量s(a,b)可以用来表示两个随机向量a和b之间的相似度,定义如下:
s(a,b)=E[f(||a-b||2)]=∫∫f(||a-b||2)p(a,b)dadb (25)
式中,||·||表示欧氏范数,p(a,b)表示随机向量a和b之间的联合概率密度。f(·)表示相似度函数,并且满足以下三个条件。
a)f(·)是在定义域[0,+∞)内的连续函数。
b)f(·)是单调递减函数,即
c)f(·)的二阶导数是非负的,即
统计相似度度量符合相似度度量的通常定义。随机向量之间的相似性越高,统计相似度度量就越大。选择f(t)为f(t)=-t时,SSM表示不同随机向量之间的负均方误差。当选择f(t)为时,SSM表示不同随机向量之间的相关熵。通过选择不同的相似函数,可以实现不同的SSM。
步骤3.2:建立鲁棒代价函数
首先假设系统模型为
Xk+1=fk(Xk)+nk (26)
式中,k表示时刻,Xk+1表示一个p维状态。表示第j个量测,fk(·)和/>表示状态转移函数和测量函数。nk和/>表示过程噪声和量测噪声。Qk和/>表示过程噪声标称协方差阵和第j个量测噪声标称协方差阵,对于本发明所介绍的SINS/DVL紧组合导航系统来说,q为4。
标准卡尔曼滤波(KF)本质上是状态预测和量测信息的加权,以获得状态的最优估计,它的加权最小二乘代价函数反映了状态和预测状态之间以及量测和预测量测之间的均方误差。而本发明则构造了反映状态与预测状态、量测与预测量测之间SSM的代价函数。
式中,和/>表示状态一步预测结果和一步预测协方差阵,/>和/>表示量测噪声方差/>和一步预测协方差阵/>的均方根分解。
为了便于求解(28)所示的代价函数,将后验分布近似为高斯分布并利用詹森(Jensen)不等式求解上式中代价函数的下界,将其作为一个新的代价函数。
式中,N(X;Xk+1,Pk+1)表示关于X的高斯分布,均值是Xk+1,方差是Pk+1。
当测量方程为非线性时,可以采用sigma点变换,如无迹变换规则或三次球面径向容积规则进行近似求解。当测量方程为线性时,(31)可写成
步骤四:应用高斯-牛顿迭代求取代价函数的最优值;
求解代价函数极值,通常情况下可以采用对代价函数求导,令导数为零的方法去计算。
由于式(33)为非线性方程,因此在本发明中使用高斯-牛顿迭代方法对(33)进行求解。
根据高斯-牛顿迭代法,对于非线性方程式(33),解的数值更新过程为
式中,上标t表示第t次迭代。迭代初值设为表示代价函数J(Xk+1,Pk+1)的近似海森(Hessian)矩阵。
其中,
整理得
上式中,滤波增益为
其中和/>相当于用来调节一步预测协方差阵和量测噪声方差的辅助参数。
估计误差为
由此可以直接得到估计误差协方差阵
通常情况下,对于SINS/DVL紧组合导航系统,陀螺和加速度计的随机漂移构成的过程噪声是具有精确协方差的高斯噪声。因此,相似度函数fx(t)选择为fx(t)=-0.5t,即采用负均方误差来度量状态与预测状态之间的相似程度。而DVL波束测量噪声则可能是由野值引起的非高斯噪声,因此相似度函数fz(t)选为ω是自由度参数,m是量测维数,在本发明中为1。
步骤五:将估计的导航误差反馈给捷联惯导。
通过鲁棒卡尔曼滤波器,可以得到估计的失准角φ,速度误差δVn和位置误差δp,假设捷联惯导纯惯性解算出的姿态阵、速度、位置分别为由于采用的是反馈校正去修正捷联惯导,惯导的误差量时刻都保持为小量,因此姿态误差的反馈修正方法为
速度误差δVn和位置误差δp直接在捷联惯导的输出中直接减去即可。
步骤六:使用湖试实验数据对所提出算法进行验证。
图3表示一次湖试实验的航行轨迹。实验中以自研捷联惯导和DVL作为待测试设备,GPS接收机和法国进口PHINS惯导组合作为基准。
图4表示所提出算法和已有的鲁棒滤波算法应用在SINS/DVL紧组合系统中的速度误差和水平定位误差。其中,RSTKF表示已有的基于Student’s t分布建模的鲁棒卡尔曼滤波器。MCKF表示已有的基于最大相关熵准则的卡尔曼滤波器。SSMKF表示已有的基于统计相似度量的卡尔曼滤波器,SSMKF与本发明所提出的鲁棒卡尔曼滤波器最大的区别在于,SSMKF只使用一个辅助参数去对所有量测的利用率做统一调节,而本发明提出的鲁棒卡尔曼滤波器则是使用多个辅助参数分别调节不同量测分量的利用率。从图4中可以看出,所提出算法的定位误差和速度误差均最小,和基于相同统计相似度量准则的SSMKF相比,所提算法的估计精度明显更好。
图5则表示实验中一段DVL波束测量值和每个波束对应的辅助参数变化情况。从图中可以看出,当出现大离群点时,DVL波束所对应的辅助参数会明显减小,从而使得波束测量噪声方差增大,降低大离群点量测在估计结果中所占的比重。而正常的DVL波束对应的辅助参数不会发生明显变化,使得所提出算法可以在降低错误量测影响的同时保证正常测量的使用。
本发明考虑SINS/DVL紧组合系统中正常的DVL波束测量信息和大误差的波束测量信息同时出现的情形,针对目前现有的组合导航系统鲁棒滤波器对量测信息处理粗糙,导致正常量测信息易丢失的问题,基于多维量测方程分解和统计相似度量提出了新的鲁棒滤波方法。
本发明在将SINS/DVL紧组合导航系统的多维量测方程进行分解的同时,引入统计相似度量(SSM)理论,协助每个波束的测量噪声方差在大测量误差出现时完成各自的自适应更新,最终保证每个波束测量信息处理的独立性。本发明可用于非理想条件下的水下航行器组合导航领域。
本发明的主要优点体现在:
(1)本发明将SINS/DVL紧组合导航系统的量测方程进行分解,并利用统计相似度量测概念建立代价函数,提高了组合导航系统鲁棒信息融合方法在非正常量测出现的情况下对正常量测信息的利用效率;
(2)本发明对非线性代价函数进行迭代求解,降低了数值计算误差;
(3)本发明在建立代价函数和对其进行求解时均将系统考虑为非线性系统,因此本发明所提出的鲁棒信息融合方法拥有着广阔的适用范围,既可用于线性系统,也可以用于非线性系统。
以上所述仅为本发明的优选实施例而已,并不用于限制本发明,对于本领域的技术人员来说,本发明可以有各种更改和变化。凡在本发明的精神和原则之内,所作的任何修改、等同替换、改进等,均应包含在本发明的保护范围之内。
Claims (1)
1.一种基于统计相似度量的组合导航鲁棒滤波方法,其特征在于,包括以下步骤:
步骤1:定义坐标系;
定义载体坐标系采用b表示,地心地固坐标系用e表示,“东-北-天”地理坐标系作为导航系采用n表示,惯性坐标系用i表示,DVL框架坐标系用d表示;
定义不同坐标系下的速度:
其中,表示捷联惯导系统在b系中的速度矢量;/>表示n系内捷联惯导系统的速度矢量;/>表示捷联惯导系统在d系中的速度矢量;/>表示d系中DVL波束的速度矢量;
步骤2:建立SINS/DVL紧组合导航系统状态方程;
选取SINS的误差作为状态,包括失准角φ、速度误差δVn、位置误差δp、陀螺常值漂移εb和加速度计常值零偏状态方程如下:
其中,εb表示陀螺常值漂移矢量;是陀螺随机漂移矢量;/>是加速度计常值零偏矢量;/>是加速度计随机零偏矢量;φ表示由俯仰、横滚和偏航失准角组成的失准角矢量;/>是方向余弦矩阵,表示从b系到n系的变换;/>表示b系下b系相对于i系的角速率矢量,对应的计算误差为/>是n系中的载体相对于地面的速度矢量,对应的误差矢量是δvn;fb是加速度计的输出;δgn是重力误差向量在n系中的投影;δp表示由纬度误差δL、经度误差δλ和高度误差δh组成的位置误差矢量;RM和RN分别表示子午线圆和本初垂直圆的曲率半径;
步骤3:建立SINS/DVL紧组合导航系统量测方程,并分解;
量测方程:
Z=HX+v
其中,表示测量噪声;I3×3是三维单位矩阵;[·×]表示向量的叉乘运算;α是波束和载体之间的水平角;/>设置为0°或45°;
DVL各个波束的测量噪声通常情况下是不相关的,其量测噪声协方差阵R=E[vvT]是一个对角阵,因此将量测方程进行等效分解:
其中,Hj表示量测矩阵的第j行;vj是相应的测量噪声,不同的DVL波束的测量噪声不相关,这里是将DVL的四个波束测量值假设为由不同的传感器分别进行测量获得;
步骤4:基于统计相似度量构建状态估计的代价函数;
其中,和/>分别表示状态一步预测结果和一步预测协方差阵;N(X;Xk+1,Pk+1)表示关于X的高斯分布,均值是Xk+1,方差是Pk+1;相似度函数fx(t)选择为fx(t)=-0.5t;相似度函数fz(t)选为/>ω是自由度参数,m是量测维数,m=1;q=4;
步骤5:应用高斯-牛顿迭代求取代价函数的最优值;
步骤5.1:初始化迭代次数t=0,设定迭代初值为
步骤5.2:计算滤波增益
其中, 和/>相当于用来调节一步预测协方差阵和量测噪声方差的辅助参数;
步骤5.3:计算后验状态
步骤5.4:计算估计误差协方差阵
步骤5.5:判断是否满足若不满足,则令t=t+1,返回步骤5.2;否则,令/>输出SINS的误差X;
步骤6:将估计的SINS的误差X反馈给SINS,进行修正;
根据估计的SINS的误差X,获取失准角φ、速度误差δVn、位置误差δp;对于SINS解算出的姿态阵速度/>位置/>姿态误差的反馈修正方法为:
速度误差δVn和位置误差δp直接在SINS的输出中直接减去即可:
Priority Applications (3)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202111115235.1A CN113819906B (zh) | 2021-09-23 | 2021-09-23 | 一种基于统计相似度量的组合导航鲁棒滤波方法 |
PCT/CN2022/093182 WO2023045357A1 (zh) | 2021-09-23 | 2022-05-17 | 一种基于统计相似度量的组合导航鲁棒滤波方法 |
US17/843,451 US20220326016A1 (en) | 2021-09-23 | 2022-06-17 | Robust Filtering Method for Integrated Navigation Based on Statistical Similarity Measure |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202111115235.1A CN113819906B (zh) | 2021-09-23 | 2021-09-23 | 一种基于统计相似度量的组合导航鲁棒滤波方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN113819906A CN113819906A (zh) | 2021-12-21 |
CN113819906B true CN113819906B (zh) | 2023-07-21 |
Family
ID=78915247
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202111115235.1A Active CN113819906B (zh) | 2021-09-23 | 2021-09-23 | 一种基于统计相似度量的组合导航鲁棒滤波方法 |
Country Status (2)
Country | Link |
---|---|
CN (1) | CN113819906B (zh) |
WO (1) | WO2023045357A1 (zh) |
Families Citing this family (12)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN113819906B (zh) * | 2021-09-23 | 2023-07-21 | 哈尔滨工程大学 | 一种基于统计相似度量的组合导航鲁棒滤波方法 |
CN115031724A (zh) * | 2022-03-21 | 2022-09-09 | 哈尔滨工程大学 | Sins/dvl紧组合系统dvl波束故障处理方法 |
CN116105731B (zh) * | 2023-04-07 | 2023-06-20 | 中国人民解放军国防科技大学 | 稀疏测距条件下的导航方法、装置、计算机设备及介质 |
CN116255988B (zh) * | 2023-05-11 | 2023-07-21 | 北京航空航天大学 | 一种基于舰船动力学组合导航复合抗干扰自适应滤波方法 |
CN116358566B (zh) * | 2023-06-01 | 2023-08-11 | 山东科技大学 | 一种基于抗差自适应因子的粗差探测组合导航方法 |
CN116680500B (zh) * | 2023-06-12 | 2024-03-22 | 哈尔滨工程大学 | 水下航行器在非高斯噪声干扰下的位置估计方法及系统 |
CN116592880B (zh) * | 2023-07-06 | 2023-11-17 | 中国科学院空天信息创新研究院 | 一种uwb_ins组合定位系统自主完好性检测方法 |
CN116817927B (zh) * | 2023-08-24 | 2023-12-22 | 北京李龚导航科技有限公司 | 双滤波器组合导航定位与测姿方法、电子设备及介质 |
CN117146830B (zh) * | 2023-10-31 | 2024-01-26 | 山东科技大学 | 一种自适应多信标航位推算和长基线的紧组合导航方法 |
CN117367410B (zh) * | 2023-12-04 | 2024-03-19 | 中南大学 | 状态估计方法、无人水下航行器及计算机可读存储介质 |
CN117606491B (zh) * | 2024-01-24 | 2024-04-26 | 中国海洋大学 | 一种自主式水下航行器的组合定位导航方法及装置 |
CN117848351B (zh) * | 2024-03-05 | 2024-05-28 | 河海大学 | 基于可信度和统计相似度量测的水下自主导航系统及方法 |
Citations (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN109724599A (zh) * | 2019-03-12 | 2019-05-07 | 哈尔滨工程大学 | 一种抗野值的鲁棒卡尔曼滤波sins/dvl组合导航方法 |
CN111947651A (zh) * | 2020-07-17 | 2020-11-17 | 中国人民解放军海军工程大学 | 水下组合导航信息融合方法、系统及自主式水下航行器 |
Family Cites Families (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JP5703846B2 (ja) * | 2011-03-02 | 2015-04-22 | 株式会社Ihi | 水中航走体の自動動作確認方法及び装置 |
CN110057382B (zh) * | 2019-04-23 | 2021-07-09 | 西北工业大学 | 一种基于发射坐标系的捷联惯导数值更新方法 |
CN112507281B (zh) * | 2020-11-19 | 2024-01-02 | 东南大学 | 一种基于双状态多因子抗差估计sins/dvl紧组合系统的方法 |
CN112525218B (zh) * | 2020-11-23 | 2023-01-03 | 哈尔滨工程大学 | 一种ins/dvl组合导航系统鲁棒智能协同校准方法 |
CN113819906B (zh) * | 2021-09-23 | 2023-07-21 | 哈尔滨工程大学 | 一种基于统计相似度量的组合导航鲁棒滤波方法 |
-
2021
- 2021-09-23 CN CN202111115235.1A patent/CN113819906B/zh active Active
-
2022
- 2022-05-17 WO PCT/CN2022/093182 patent/WO2023045357A1/zh unknown
Patent Citations (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN109724599A (zh) * | 2019-03-12 | 2019-05-07 | 哈尔滨工程大学 | 一种抗野值的鲁棒卡尔曼滤波sins/dvl组合导航方法 |
CN111947651A (zh) * | 2020-07-17 | 2020-11-17 | 中国人民解放军海军工程大学 | 水下组合导航信息融合方法、系统及自主式水下航行器 |
Non-Patent Citations (1)
Title |
---|
An Improved Robust Kalman Filter for SINS/DVL Tightly Integrated Navigation System;Bo Xu etc.;《IEEE TRANSACTIONS ON INSTRUMENTATION AND MEASUREMENT》;全文 * |
Also Published As
Publication number | Publication date |
---|---|
WO2023045357A1 (zh) | 2023-03-30 |
CN113819906A (zh) | 2021-12-21 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN113819906B (zh) | 一种基于统计相似度量的组合导航鲁棒滤波方法 | |
US11237276B2 (en) | System and method for gaussian process enhanced GNSS corrections generation | |
CN110823217B (zh) | 一种基于自适应联邦强跟踪滤波的组合导航容错方法 | |
CN112284384B (zh) | 考虑量测异常的集群式多深海潜航器的协同定位方法 | |
CN112525218B (zh) | 一种ins/dvl组合导航系统鲁棒智能协同校准方法 | |
CN106679693A (zh) | 一种基于故障检测的矢量信息分配自适应联邦滤波方法 | |
US20240142637A1 (en) | System and method for gaussian process enhanced gnss corrections generation | |
CN110567454B (zh) | 一种复杂环境下sins/dvl紧组合导航方法 | |
CN110567455B (zh) | 一种求积更新容积卡尔曼滤波的紧组合导航方法 | |
Xu et al. | An improved robust Kalman filter for SINS/DVL tightly integrated navigation system | |
CN112114341B (zh) | 低轨卫星协同测频无源定位方法 | |
CN110703355B (zh) | 一种星载加速度计的校准方法及装置 | |
CA2699137A1 (en) | Hybrid inertial system with non-linear behaviour and associated method of hybridization by multi-hypothesis filtering | |
CN110221278B (zh) | 一种基于多传感器组合的合成孔径声呐运动补偿方法 | |
CN110207721B (zh) | 一种考虑残差分布的无效地形匹配结果识别方法 | |
CN115855049A (zh) | 基于粒子群优化鲁棒滤波的sins/dvl导航方法 | |
Bian et al. | Study on GPS attitude determination system aided INS using adaptive Kalman filter | |
CN112697154A (zh) | 一种基于矢量分配的自适应多源融合导航方法 | |
CN116358566B (zh) | 一种基于抗差自适应因子的粗差探测组合导航方法 | |
CN111290008A (zh) | 一种动态自适应扩展卡尔曼滤波容错算法 | |
CN111198402A (zh) | 基于轨道掩膜微分算子的地球重力场模型建模方法 | |
CN116182853A (zh) | 一种船舶自适应组合导航设备及方法 | |
CN114279443B (zh) | 一种基于最大熵自适应抗差估计的usbl斜距修正方法 | |
CN115031729A (zh) | Sins/dvl/usbl水下紧组合导航方法及装置、水下载体控制设备 | |
Zhang et al. | An outlier-robust Rao–Blackwellized particle filter for underwater terrain-aided navigation |
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 |