CN111380519B - 一种超短基线/捷联惯性导航松组合的导航误差校正方法 - Google Patents
一种超短基线/捷联惯性导航松组合的导航误差校正方法 Download PDFInfo
- Publication number
- CN111380519B CN111380519B CN202010144251.2A CN202010144251A CN111380519B CN 111380519 B CN111380519 B CN 111380519B CN 202010144251 A CN202010144251 A CN 202010144251A CN 111380519 B CN111380519 B CN 111380519B
- Authority
- CN
- China
- Prior art keywords
- coordinate system
- navigation
- underwater vehicle
- strapdown inertial
- axis
- 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
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/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/005—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 with correlation of navigation data from several sources, e.g. map or contour matching
-
- 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/18—Stabilised platforms, e.g. by gyroscope
-
- 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
-
- 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
- G01S5/00—Position-fixing by co-ordinating two or more direction or position line determinations; Position-fixing by co-ordinating two or more distance determinations
- G01S5/18—Position-fixing by co-ordinating two or more direction or position line determinations; Position-fixing by co-ordinating two or more distance determinations using ultrasonic, sonic, or infrasonic waves
- G01S5/22—Position of source determined by co-ordinating a plurality of position lines defined by path-difference measurements
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Computer Networks & Wireless Communication (AREA)
- Measurement Of Velocity Or Position Using Acoustic Or Ultrasonic Waves (AREA)
- Navigation (AREA)
Abstract
一种超短基线/捷联惯性导航松组合的导航误差校正方法,它属于组合导航及水声定位技术领域。本发明解决了在基阵开角较大时,水下航行器的捷联惯性导航需要运动至超短基阵有效工作范围内进行误差校正,导致水下航行器的工作效率降低的问题。当超短基线定位***有实数解时,可以得到声学基阵与信标的相对深度和斜距的比值,即基阵开角的余弦值,以该值控制组合导航的滤波增益,能够减小基阵开角比较大时的低质量观测信息在状态更新中所占的比重。通过在外部对滤波器的增益进行控制,能有效抑制捷联惯性导航误差,并且保证了在基阵开角较大情况下的水下航行器的工作效率。本发明可以应用于组合导航及水声定位领域。
Description
技术领域
本发明属于组合导航及水声定位技术领域,具体涉及一种超短基线/捷联惯性导航松组合的滤波增益控制方法。
背景技术
随着基阵开角的增大,超短基线的定位误差变大,组合导航精度受到影响。换言之,传统的超短基线和捷联惯性导航的组合只能在以某一基阵开角为半锥角的圆锥内部及表面进行,水下航行器在其它位置区域时,只能依靠捷联惯性导航,而纯惯性导航存在着误差随时间积分累积的缺点,使得水下航行器需要每隔一段时间运动至超短基线的有效工作范围内,以此来校正惯性导航的累积误差,这样就大大降低了水下航行器的工作效率。
发明内容
本发明的目的是为解决在基阵开角较大时,水下航行器的捷联惯性导航需要运动至超短基阵有效工作范围内进行误差校正,导致水下航行器的工作效率降低的问题,而提出了一种超短基线/捷联惯性导航松组合的导航误差校正方法。
本发明为解决上述技术问题采取的技术方案是:一种超短基线/捷联惯性导航松组合的导航误差校正方法,该方法包括以下步骤:
步骤一、将超短基线声学基阵倒置安装在水下航行器上,捷联惯性导航的陀螺组件和加速度计组件固联在水下航行器上,水面布放一个同步信标;信标在导航坐标系下的位置通过GPS获得;
所述导航坐标系是指“东北天”地理坐标系,该地理坐标系以水下航行器的质心为原点o,x轴指向地理东向,y轴指向地理北向,z轴垂直于xoy平面指向天向,x轴、y轴和z轴构成右手坐标系;
步骤二、通过测量声信号在超短基线声学基阵与信标之间的单程传播时延及有效声速,在基阵坐标系下,计算信标的位置以及超短基线声学基阵与信标的相对深度,再计算相对深度与声信号在水下航行器和信标之间传播距离的比值,计算出的比值即为基阵开角的余弦值;
所述基阵坐标系是指以超短基线声学基阵中心为原点Oa,沿超短基线声学基阵平面指向水下航行器的艏向方向为Ya轴,Za轴垂直于基阵平面向上,Xa轴与Ya轴、Za轴构成右手坐标系;
步骤五、根据步骤二计算出的基阵开角余弦值调整扩展卡尔曼滤波器的滤波增益,实现状态更新;
步骤六、根据步骤五更新后的状态值,修正捷联惯性导航的输出,并重置扩展卡尔曼滤波器的状态。
本发明的有益效果是:本发明提出了一种超短基线/捷联惯性导航松组合的导航误差校正方法,当超短基线定位***定位有效时,可以得到声学基阵与信标的相对深度和斜距的比值,即基阵开角的余弦值,以该值控制组合导航的滤波增益,能够减小基阵开角比较大时的低质量观测信息在状态更新中所占的比重。
通过在外部对滤波器的增益进行控制,能有效抑制捷联惯性导航误差,并且保证了在基阵开角较大情况下的水下航行器的工作效率。
附图说明
图1是超短基线定位有效时超短基线/捷联惯性导航松组合的导航误差校正方法的原理图。
具体实施方式
具体实施方式一:如图1所示,本实施方式所述的一种超短基线/捷联惯性导航松组合的导航误差校正方法,该方法包括以下步骤:
步骤一、将超短基线声学基阵倒置安装在水下航行器上,捷联惯性导航的陀螺组件和加速度计组件固联在水下航行器上,水面布放一个同步信标;信标在导航坐标系下的位置通过GPS获得;
所述导航坐标系是指“东北天”(“ENU”)地理坐标系,该地理坐标系以水下航行器的质心为原点o,x轴指向地理东向,y轴指向地理北向,z轴垂直于xoy平面指向天向,x轴、y轴和z轴构成右手坐标系;
步骤二、通过测量声信号在超短基线声学基阵与信标之间的单程传播时延及有效声速,在基阵坐标系下,计算信标的位置以及超短基线声学基阵与信标的相对深度,再计算相对深度与声信号在水下航行器和信标之间传播距离的比值,计算出的比值即为基阵开角的余弦值;
所述基阵坐标系是指以超短基线声学基阵中心为原点Oa,沿超短基线声学基阵平面指向水下航行器的艏向方向为Ya轴,Za轴垂直于基阵平面向上,Xa轴与Ya轴、Za轴构成右手坐标系;
所述捷联惯性导航计算导航坐标系是指:捷联惯性导航***通过解算水下航行器的姿态信息实现对“东北天”地理坐标系的复现;
步骤五、根据步骤二计算出的基阵开角余弦值调整扩展卡尔曼滤波器的滤波增益,实现状态更新;
步骤六、根据步骤五更新后的状态值,修正捷联惯性导航的输出,并重置扩展卡尔曼滤波器的状态。
重置扩展卡尔曼滤波器的状态后,再重复步骤二至步骤五的过程,利用更新后的状态值修正捷联惯性导航的输出,不断的对捷联惯性导航的输出进行修正。
在任意基阵开角下,本发明都可以保证组合导航精度和航行器工作效率。
具体实施方式二:本实施方式与具体实施方式一不同的是:所述有效声速为声源和超短基线声学基阵的几何距离与声音在两点间传播时间的比值。
具体实施方式三:本实施方式与具体实施方式一不同的是:所述基阵开角是指在基阵坐标系下,超短基线声学基阵同信标的连线与Za轴正向的夹角。
具体实施方式四:本实施方式与具体实施方式一不同的是:所述步骤二中,在基阵坐标系下,信标的位置为[Xa′ Ya′ Za′]T,基阵开角的余弦值为cosθZ;
其中:R是声信号在水下航行器和信标之间的传播距离,Xa′为信标在Xa轴方向的位置,Ya′为信标在Ya轴方向的位置,Za′为信标在Za轴方向的位置;cosθX是信标在Xa轴方向的位置与R的比值,cosθY是信标在Ya轴方向的位置与R的比值;
cos2θX+cos2θY+cos2θZ=1
其中:c是水中声速,τ是超短基线声学基阵各个基元测量的单程传播时延均值,单程传播距离为水下航行器相对于信标的距离,τX和τY分别是两两基元的传播时延差沿基阵坐标系Xa轴和Ya轴的分量,dX和dY是对应的两基元在Xa轴和Ya轴方向上的位置差。
具体实施方式五:本实施方式与具体实施方式四不同的是:所述步骤三的具体过程为:
捷联惯性导航提供的水下航行器姿态信息包括水下航行器的航向角A、俯仰角K和横滚角ψ;
建立载体坐标系obxbybzb,载体坐标系的坐标原点ob位于水下航行器的质心,坐标轴xb的正方向沿水下航行器的横轴指向右,坐标轴yb的正方向沿水下航行器的纵轴指向前,坐标轴zb的正方向沿水下航行器的立轴指向上,载体坐标系的定义满足右手定则;
基阵坐标系相对于载体坐标系的角度安装偏差分别为α、β和γ(基阵坐标系的三个坐标轴与载体坐标系的三个坐标轴的角度偏差),基阵坐标系原点相对于载体坐标系原点的位置偏差为:(ΔXb、ΔYb和ΔZb均为载体坐标系下分量),ΔXb、ΔYb和ΔZb为中的分量,上角标T代表转置;
捷联惯性导航计算的导航坐标系n'与真实导航坐标系n(建立的导航坐标系)的三个坐标轴存在角度误差φ:φ=[φE φN φU]T,称φ为捷联惯性导航失准角误差,φE、φN和φU均为φ中的分量,捷联惯性导航计算的导航坐标系n'与真实导航坐标系n之间的转换矩阵可近似为
其中:φ×为中间变量,I为单位矩阵;
本发明中,未特别限定的导航坐标系,均是指建立的导航坐标系。
α、β和γ分别是基阵坐标系的三个坐标轴相对于载体坐标系的三个坐标轴的角度偏差;
航向角为载体(水下航行器)艏向与地理北向的夹角,定义为北偏西为正,角度范围(-180°,180°];横滚角γ为载体立轴zb与横轴xb轴所在铅垂面的夹角,载体向右倾斜时角度为正,角度范围(-180°,180°];俯仰角θ为载体纵轴yb与其水平投影线之间的夹角,当载体抬头时为正,角度范围(-90°,90°]。
捷联惯性导航解算水下航行器在导航坐标系下的姿态、速度和位置。结合声学基阵的安装偏差校准结果和姿态信息,将超短基线解算的信标在声学基阵坐标系下的相对位置进行转换,得到水下航行器在计算导航坐标系下位置。
具体实施方式六:本实施方式与具体实施方式五不同的是:所述步骤四的具体过程为:
将捷联惯性导航输出的水下航行器位置表示为纬度L、经度λ和高度h的形式,再将纬度L、经度λ和高度h转换为地球直角坐标,其中,地球直角坐标系以地心为原点,xe轴指向本初子午线与赤道交点,ye轴指向90°经线与赤道交点,ze轴与xe轴、ye轴构成右手坐标系,则
式中,xe′、ye′和ze′分别为捷联惯性导航输出的水下航行器在地球直角坐标系的xe轴、ye轴和ze轴方向的位置,RN为地球卯酉圈主曲率半径,Re为地球半径,e为地球偏心率,a和b分别是椭圆长轴和短轴半径;
上式的微分转换关系为:
地球直角坐标系到捷联惯性导航计算导航坐标系的转换矩阵为:
扩展卡尔曼滤波器的状态变量由捷联惯性导航失准角误差φ=[φE φN φU]T,捷联惯性导航速度误差δν=[δvE δvN δvU]T,捷联惯性导航位置误差δp=[δL δλ δh]T,捷联惯性导航陀螺漂移误差ε=[εx εy εz]T,捷联惯性导航加速度计偏置误差组成;
扩展卡尔曼滤波器的状态方程为:
Xk+1=Fk+1/kXk+wk+1
其中:Xk为k时刻扩展卡尔曼滤波器的状态变量,Xk+1为k+1时刻扩展卡尔曼滤波器的状态变量,Fk+1/k为状态转移矩阵,由捷联惯性导航***的误差方程获得,wk+1为扩展卡尔曼滤波***过程噪声序列,通常为高斯白噪声形式;
则扩展卡尔曼滤波的观测量Z为:
扩展卡尔曼滤波的观测方程Zk+1为:
Zk+1=Hk+1Xk+1+vk+1
其中:vk+1为扩展卡尔曼滤波***观测噪声序列,通常为高斯白噪声形式,Hk+1为观测矩阵。
组合导航一般有反馈校正的闭环方式和输出修正的开环方式,前者工程实现复杂,当滤波器有故障时会直接影响捷联惯性导航的输出,而输出修正不涉及独立的导航***内部,容错能力较强,因此本发明采用输出修正方式。
具体实施方式七:本实施方式与具体实施方式六不同的是:所述步骤五的具体过程为:
在超短基线声学基阵定位无效时,没有量测信息进入扩展卡尔曼滤波器,此时,扩展卡尔曼滤波器只进行时间更新:
根据Pk+1/k=Fk+1/kPkFT k+1/k+Qk计算k+1时刻的状态预测误差协方差矩阵Pk+1/k,其中Pk为k时刻的状态估计误差协方差矩阵,Qk为k时刻的***过程噪声协方差矩阵;
在超短基线声学基阵定位有效时,量测信息进入扩展卡尔曼滤波器,扩展卡尔曼滤波器完成量测更新:
根据Kk+1=PkHT k+1(Hk+1PkHT k+1+Rk+1)-1计算k+1时刻的滤波器增益Kk+1,其中,Rk+1和Hk+1分别是k+1时刻的***观测噪声协方差矩阵和观测矩阵;上角标-1代表矩阵的逆;
根据Pk+1=(I-Kk+1Hk+1)Pk计算k+1时刻的状态估计误差协方差矩阵Pk+1;
所述基于位置信息的扩展卡尔滤波器是指以捷联惯性导航的姿态误差、速度误差、位置误差和陀螺漂移、加速度计偏置为状态变量,以超短基线和捷联惯性导航输出的水下航行器位置之差作为观测量对状态变量进行估计。
所述扩展卡尔滤波器是指将非线性的状态方程和观测方程进行线性近似处理。
所述卡尔滤波器是指在最小均方误差准则下的线性贝叶斯估计;卡尔滤波采用状态方程描述相邻时刻状态变量的关系,采用观测方程描述状态变量和观测量的关系。
所述调整滤波增益是指利用基阵开角的余弦值控制理论计算的滤波器增益,适当地减小在大基阵开角情况下的滤波增益,降低低质量的超短基线声学定位结果对状态变量估计精度的影响程度。
所述修正捷联惯性导航输出是指在捷联惯导原导航输出上减去状态变量的估计值,得到新的惯性导航输出。
所述重置滤波状态变量是指,在修正捷联惯性导航输出后,理论上认为此时捷联惯导输出的导航信息不存在误差,因此滤波状态变量为零。
|cosθz|的取值范围为[0 1],在大开角的情况下,|cosθz|能够适当调整滤波增益,减小低质量的观测信息对滤波状态更新的影响,提高组合导航精度。
本发明中,扩展卡尔曼滤波器的状态方程是由捷联惯性导航***的误差方程构成,但不仅限于此,如在状态方程中加入超短基线的定位误差、测距误差和安装偏差等。
本发明的上述算例仅为详细地说明本发明的计算模型和计算流程,而并非是对本发明的实施方式的限定。对于所属领域的普通技术人员来说,在上述说明的基础上还可以做出其它不同形式的变化或变动,这里无法对所有的实施方式予以穷举,凡是属于本发明的技术方案所引伸出的显而易见的变化或变动仍处于本发明的保护范围之列。
Claims (7)
1.一种超短基线/捷联惯性导航松组合的导航误差校正方法,其特征在于,该方法包括以下步骤:
步骤一、将超短基线声学基阵倒置安装在水下航行器上,捷联惯性导航的陀螺组件和加速度计组件固联在水下航行器上,水面布放一个同步信标;信标在导航坐标系下的位置通过GPS获得;
所述导航坐标系是指“东北天”地理坐标系,该地理坐标系以水下航行器的质心为原点o,x轴指向地理东向,y轴指向地理北向,z轴垂直于xoy平面指向天向,x轴、y轴和z轴构成右手坐标系;
步骤二、通过测量声信号在超短基线声学基阵与信标之间的单程传播时延及有效声速,在基阵坐标系下,计算信标的位置以及超短基线声学基阵与信标的相对深度,再计算相对深度与声信号在水下航行器和信标之间传播距离的比值,计算出的比值即为基阵开角的余弦值;
所述基阵坐标系是指以超短基线声学基阵中心为原点Oa,沿超短基线声学基阵平面指向水下航行器的艏向方向为Ya轴,Za轴垂直于基阵平面向上,Xa轴与Ya轴、Za轴构成右手坐标系;
步骤五、根据步骤二计算出的基阵开角余弦值调整扩展卡尔曼滤波器的滤波增益,实现状态更新;
步骤六、根据步骤五更新后的状态值,修正捷联惯性导航的输出,并重置扩展卡尔曼滤波器的状态。
2.根据权利要求1所述的一种超短基线/捷联惯性导航松组合的导航误差校正方法,其特征在于,所述有效声速为声源和超短基线声学基阵的几何距离与声信号在两点间传播时间的比值。
3.根据权利要求1所述的一种超短基线/捷联惯性导航松组合的导航误差校正方法,其特征在于,所述基阵开角是指在基阵坐标系下,超短基线声学基阵同信标的连线与Za轴正向的夹角。
4.根据权利要求1所述的一种超短基线/捷联惯性导航松组合的导航误差校正方法,其特征在于,所述步骤二中,在基阵坐标系下,信标的位置为[X′a Y′a Z′a]T,基阵开角的余弦值为cosθZ;
其中:R是声信号在水下航行器和信标之间的传播距离,X′a为信标在Xa轴方向的位置,Y′a为信标在Ya轴方向的位置,Z′a为信标在Za轴方向的位置;cosθX是信标在Xa轴方向的位置与R的比值,cosθY是信标在Ya轴方向的位置与R的比值;
cos2θX+cos2θY+cos2θZ=1
5.根据权利要求4所述的一种超短基线/捷联惯性导航松组合的导航误差校正方法,其特征在于,所述步骤三的具体过程为:
捷联惯性导航提供的水下航行器姿态信息包括水下航行器的航向角A、俯仰角K和横滚角ψ;
建立载体坐标系obxbybzb,载体坐标系的坐标原点ob位于水下航行器的质心,坐标轴xb的正方向沿水下航行器的横轴指向右,坐标轴yb的正方向沿水下航行器的纵轴指向前,坐标轴zb的正方向沿水下航行器的立轴指向上,载体坐标系的定义满足右手定则;
捷联惯性导航计算的导航坐标系n'与真实导航坐标系n的三个坐标轴存在角度误差φ:φ=[φE φN φU]T,称φ为捷联惯性导航失准角误差,φE、φN和φU均为φ中的分量,捷联惯性导航计算的导航坐标系n'与真实导航坐标系n之间的转换矩阵为
其中:φ×为中间变量,I为单位矩阵;
6.根据权利要求5所述的一种超短基线/捷联惯性导航松组合的导航误差校正方法,其特征在于,所述步骤四的具体过程为:
将捷联惯性导航输出的水下航行器位置表示为纬度L、经度λ和高度h的形式,再将纬度L、经度λ和高度h转换为地球直角坐标,其中,地球直角坐标系以地心为原点,xe轴指向本初子午线与赤道交点,ye轴指向90°经线与赤道交点,ze轴与xe轴、ye轴构成右手坐标系,则
式中,x′e、y′e和z′e分别为捷联惯性导航输出的水下航行器在地球直角坐标系的xe轴、ye轴和ze轴方向的位置,RN为地球卯酉圈主曲率半径,Re为地球半径,e为地球偏心率,a和b分别是椭圆长轴和短轴半径;
上式的微分转换关系为:
地球直角坐标系到捷联惯性导航计算导航坐标系的转换矩阵为:
扩展卡尔曼滤波器的状态变量由捷联惯性导航失准角误差φ=[φE φN φU]T,捷联惯性导航速度误差δν=[δvE δvN δvU]T,捷联惯性导航位置误差δp=[δL δλ δh]T,捷联惯性导航陀螺漂移误差ε=[εx εy εz]T,捷联惯性导航加速度计偏置误差组成;
扩展卡尔曼滤波器的状态方程为:
Xk+1=Fk+1/kXk+wk+1
其中:Xk为k时刻扩展卡尔曼滤波器的状态变量,Xk+1为k+1时刻扩展卡尔曼滤波器的状态变量,Fk+1/k为状态转移矩阵,wk+1为扩展卡尔曼滤波***过程噪声序列;
则扩展卡尔曼滤波的观测量Z为:
扩展卡尔曼滤波的观测方程Zk+1为:
Zk+1=Hk+1Xk+1+vk+1
其中:vk+1为扩展卡尔曼滤波***观测噪声序列,Hk+1为观测矩阵。
7.根据权利要求6所述的一种超短基线/捷联惯性导航松组合的导航误差校正方法,其特征在于,所述步骤五的具体过程为:
在超短基线声学基阵定位无效时,没有量测信息进入扩展卡尔曼滤波器,扩展卡尔曼滤波器只进行时间更新:
根据Pk+1/k=Fk+1/kPkFT k+1/k+Qk计算k+1时刻的状态预测误差协方差矩阵Pk+1/k,其中Pk为k时刻的状态估计误差协方差矩阵,Qk为k时刻的***过程噪声协方差矩阵;
在超短基线声学基阵定位有效时,量测信息进入扩展卡尔曼滤波器,扩展卡尔曼滤波器完成量测更新:
根据Kk+1=PkHT k+1(Hk+1PkHT k+1+Rk+1)-1计算k+1时刻的滤波器增益Kk+1,其中,Rk+1和Hk+1分别是k+1时刻的***观测噪声协方差矩阵和观测矩阵;
根据Pk+1=(I-Kk+1Hk+1)Pk计算k+1时刻的状态估计误差协方差矩阵Pk+1;
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202010144251.2A CN111380519B (zh) | 2020-03-04 | 2020-03-04 | 一种超短基线/捷联惯性导航松组合的导航误差校正方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202010144251.2A CN111380519B (zh) | 2020-03-04 | 2020-03-04 | 一种超短基线/捷联惯性导航松组合的导航误差校正方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN111380519A CN111380519A (zh) | 2020-07-07 |
CN111380519B true CN111380519B (zh) | 2021-08-31 |
Family
ID=71222696
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202010144251.2A Active CN111380519B (zh) | 2020-03-04 | 2020-03-04 | 一种超短基线/捷联惯性导航松组合的导航误差校正方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN111380519B (zh) |
Families Citing this family (3)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN112836889B (zh) * | 2021-02-19 | 2024-07-19 | 鹏城实验室 | 路径优化方法、水下航行器以及计算机可读存储介质 |
CN113176539B (zh) * | 2021-04-25 | 2022-09-09 | 哈尔滨工程大学 | 一种水声信号噪声多级抑制与稳健定位***及定位方法 |
CN116068540B (zh) * | 2023-02-17 | 2023-09-12 | 哈尔滨工程大学 | 一种声学多普勒测速径向波束角校正方法 |
Family Cites Families (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US8130142B2 (en) * | 2009-09-21 | 2012-03-06 | Appareo Systems, Llc | GNSS ultra-short baseline heading determination system and method |
CN105509765B (zh) * | 2014-09-23 | 2018-07-20 | 北京自动化控制设备研究所 | 一种惯性/dvl/usbl安装误差标定方法 |
CN105486313A (zh) * | 2016-02-03 | 2016-04-13 | 东南大学 | 一种基于usbl辅助低成本sins***的定位方法 |
CN109737956B (zh) * | 2018-12-17 | 2022-06-21 | 东南大学 | 一种基于双应答器的sins/usbl相位差紧组合导航定位方法 |
CN109828296A (zh) * | 2019-03-08 | 2019-05-31 | 哈尔滨工程大学 | 一种ins/usbl非线性紧组合综合校正方法 |
-
2020
- 2020-03-04 CN CN202010144251.2A patent/CN111380519B/zh active Active
Also Published As
Publication number | Publication date |
---|---|
CN111380519A (zh) | 2020-07-07 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN111380519B (zh) | 一种超短基线/捷联惯性导航松组合的导航误差校正方法 | |
CN110487301B (zh) | 一种雷达辅助机载捷联惯性导航***初始对准方法 | |
CN111380518B (zh) | 一种引入径向速度的sins/usbl紧组合导航定位方法 | |
AU2020103939A4 (en) | Polar Integrated Navigation Algorithm of SINS / GPS Based on Grid Framework | |
CN101949703B (zh) | 一种捷联惯性/卫星组合导航滤波方法 | |
CN109211269B (zh) | 一种双轴旋转惯导***姿态角误差标定方法 | |
CN107806874B (zh) | 一种视觉辅助的捷联惯导极区初始对准方法 | |
CN108444476B (zh) | 一种考虑水声通信延迟的多水下无人航行器极区协同导航方法 | |
CN101788296A (zh) | 一种sins/cns深组合导航***及其实现方法 | |
CN109443385B (zh) | 一种动中通天线的惯导安装误差自动标定方法 | |
CN115200574A (zh) | 一种地球椭球模型下的极区横向组合导航方法 | |
CN111207773B (zh) | 一种用于仿生偏振光导航的姿态无约束优化求解方法 | |
CN110207698B (zh) | 一种极区格网惯导/超短基线紧组合导航方法 | |
CN111025229B (zh) | 一种水下机器人纯方位目标估计方法 | |
CN110388942B (zh) | 一种基于角度和速度增量的车载姿态精对准*** | |
CN104634348B (zh) | 组合导航中的姿态角计算方法 | |
CN113155134B (zh) | 一种基于惯性信息辅助的水声信道跟踪与预测方法 | |
CN112083425A (zh) | 一种引入径向速度的sins/lbl紧组合导航方法 | |
CN112833878A (zh) | 近地面多源天文自主导航方法 | |
CN111380520B (zh) | 一种引入径向速度的sins/usbl松组合导航定位方法 | |
CN114061574B (zh) | 一种基于位置不变约束及零速校正的采煤机定姿定向方法 | |
CN111721284B (zh) | 一种无源模式下的sins/usbl组合导航定位方法 | |
CN113252029B (zh) | 一种基于光学陀螺量测信息的天文导航姿态传递方法 | |
CN109471103A (zh) | 一种弹载双基sar数据融合定位误差修正方法 | |
CN111380517B (zh) | 一种基于usbl软件接收机的sins/usbl深组合导航定位方法 |
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 |