CN115265528A - 基于未知输入观测器的组合导航系统鲁棒抗扰滤波方法 - Google Patents
基于未知输入观测器的组合导航系统鲁棒抗扰滤波方法 Download PDFInfo
- Publication number
- CN115265528A CN115265528A CN202210748607.2A CN202210748607A CN115265528A CN 115265528 A CN115265528 A CN 115265528A CN 202210748607 A CN202210748607 A CN 202210748607A CN 115265528 A CN115265528 A CN 115265528A
- Authority
- CN
- China
- Prior art keywords
- representing
- unknown input
- interference
- error
- 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.)
- Pending
Links
- 238000001914 filtration Methods 0.000 title claims abstract description 72
- 238000000034 method Methods 0.000 title claims abstract description 53
- 238000005259 measurement Methods 0.000 claims abstract description 64
- 239000011159 matrix material Substances 0.000 claims abstract description 38
- 239000013598 vector Substances 0.000 claims abstract description 13
- 230000036039 immunity Effects 0.000 claims abstract description 9
- 230000000875 corresponding effect Effects 0.000 claims description 15
- 238000000354 decomposition reaction Methods 0.000 claims description 8
- 230000009897 systematic effect Effects 0.000 claims description 8
- 150000001875 compounds Chemical class 0.000 claims description 7
- 238000012546 transfer Methods 0.000 claims description 7
- 239000000126 substance Substances 0.000 claims description 6
- 238000004458 analytical method Methods 0.000 claims description 3
- 230000002596 correlated effect Effects 0.000 claims description 3
- 238000005516 engineering process Methods 0.000 claims description 3
- 238000005070 sampling Methods 0.000 claims description 3
- 239000000203 mixture Substances 0.000 claims description 2
- 230000000694 effects Effects 0.000 abstract description 5
- 238000004422 calculation algorithm Methods 0.000 description 17
- 238000004088 simulation Methods 0.000 description 13
- 230000003044 adaptive effect Effects 0.000 description 10
- 238000012937 correction Methods 0.000 description 6
- 230000004927 fusion Effects 0.000 description 5
- 238000002474 experimental method Methods 0.000 description 4
- 238000011160 research Methods 0.000 description 4
- 238000012986 modification Methods 0.000 description 3
- 230000004048 modification Effects 0.000 description 3
- 238000012545 processing Methods 0.000 description 3
- 230000002159 abnormal effect Effects 0.000 description 2
- 230000006978 adaptation Effects 0.000 description 2
- 238000004364 calculation method Methods 0.000 description 2
- 238000010586 diagram Methods 0.000 description 2
- 230000035772 mutation Effects 0.000 description 2
- 238000005295 random walk Methods 0.000 description 2
- 238000012795 verification Methods 0.000 description 2
- 230000001133 acceleration Effects 0.000 description 1
- 238000005094 computer simulation Methods 0.000 description 1
- 238000013401 experimental design Methods 0.000 description 1
- 230000005764 inhibitory process Effects 0.000 description 1
- 238000012544 monitoring process Methods 0.000 description 1
- 230000001629 suppression Effects 0.000 description 1
- 238000012360 testing method Methods 0.000 description 1
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/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/183—Compensation of inertial measurements, e.g. for temperature effects
-
- 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/48—Determining position by combining or switching between position solutions derived from the satellite radio beacon positioning system and position solutions derived from a further system
- G01S19/49—Determining position by combining or switching between position solutions derived from the satellite radio beacon positioning system and position solutions derived from a further system whereby the further system is an inertial position system, e.g. loosely-coupled
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)
- Navigation (AREA)
- Position Fixing By Use Of Radio Waves (AREA)
Abstract
本发明公开了基于未知输入观测器的组合导航系统鲁棒抗扰滤波方法,首先建立了在系统状态和量测信号中同时存在不同未知输入项Mk‑ 1dk‑1和Nkek的SINS/GNSS组合导航滤波模型,其中Mk‑1∈Rn×p和Nk∈Rm×m表示列满秩的未知输入干扰的系数矩阵,dk‑1∈Rp和ek∈Rm分别是两种未知输入干扰向量;然后,根据新息以及状态历史时刻值建立了未知输入估计器,并由指数加权滑动窗实现判断未知输入存在位置以及解耦成两种未知输入;最后,利用未知输入估计器得到的估计值对系统状态和量测进行修正,本发明提出的滤波方法能够对突变型、斜坡型、交叠型、不同特征随机噪声等类型的未知输入干扰进行准确估计,具有更好的滤波估计效果和更强的鲁棒抗扰性,能够提高组合导航系统针对未知输入干扰的抗扰能力。
Description
技术领域
本发明涉及SINS/GNSS组合导航系统信息融合和滤波领域,尤其是一种基于未知输入观测器的组合导航系统鲁棒抗扰滤波方法。
背景技术
组合导航,是将多种导航系统按某种方式组合为一种系统的导航方式,从出现至今因其结合多种传感器数据融合以获得更高导航定位精度的特性而一直属于导航领域的重点研究方向。高精度导航定位效果一方面取决于组合导航中各传感器的信号质量,另一方面则与组合导航所采用的融合滤波算法的精度和鲁棒性密不可分。因此在保证传感器精度的前提下,设计合适的滤波算法提高组合导航系统对外界干扰和内部扰动的鲁棒性,使系统能够面对各种可能的干扰仍能稳定运行具有重要意义和研究价值。
近年来,随着组合导航系统的广泛应用,人们开始重视起组合导航系统在实际应用中遇到的各种问题,比如载体运行时环境变化引起的外部扰动、GPS等量测传感器信号易受干扰、系统运行时各传感器内部产生的未知粗差以及滤波解算时系统建模不准确引入的误差等等。为了解决这些问题,组合导航鲁棒滤波算法逐渐成为学界重点研究领域之一。但迄今为止的各类研究中,学者们主要针对噪声特性变化或噪声特性未知情况下的组合导航系统鲁棒滤波进行了研究,并且研究主要集中在量测信号出现异常时的情况,没有考虑系统模型除噪声以外的不确定性,也没有充分考虑到实际应用中量测与系统本身同时出现异常的情况。
未知输入滤波指在系统含有模型完全未知的输入时,对状态和未知输入同时实现估计的滤波方法,其常用于环境监测、导航与定位等领域中。在导航领域以外,针对未知输入滤波的研究已有了许多成果,从最初的Kitanidis提出的无偏最小方差线性估计方法,到后来Gillijns等人不断完善总结的三步迭代滤波算法,以及近年来的无导数卡尔曼滤波算法、扩展Kitanidis卡尔曼滤波器、基于无迹卡尔曼滤波的未知输入无偏最小方差估计方法、基于自校准技术的无迹卡尔曼滤波算法等。上述各类方法,大多对系统有诸多要求,例如系统完全可观、未知输入维数要小于量测维数以及滤波过程中各环节频率一致等限制条件,而这些条件在常用的SINS/GPS组合导航系统中往往是无法满足的。
发明内容
为了克服现有技术中存在的上述问题,本发明提出一种基于未知输入观测器的组合导航系统鲁棒抗扰滤波方法。
本发明解决其技术问题所采用的技术方案是:基于未知输入观测器的组合导航系统鲁棒抗扰滤波方法,包括以下步骤:
步骤一:考虑未知输入dk-1∈Rp和ek∈Rm的存在,建立SINS/GNSS组合导航系统的滤波模型;
步骤二:根据量测矩阵Hk将dk-1分解为可观和不可观的两种未知输入;
步骤三:根据指数加权滑动窗估计值、历史均值与方差判断未知输入存在位置;
步骤四:利用新息和状态自校准技术设计出未知输入估计器并对系统状态和量测进行修正。
上述的一种基于未知输入观测器的组合导航系统鲁棒抗扰滤波方法,所述步骤1具体方法为:
Xk=Fk|k-1Xk-1+Mk-1dk-1+Γk-1wk-1
Zk=HkXk+Nkek+vk
式中Xk表示系统的状态,Zk表示系统量测,dk-1和ek表示未知输入干扰向量;Fk|k-1表示SINS的误差一步转移矩阵,由SINS的各误差方程(组)系数决定;Hk为系统量测矩阵;Mk-1和Nk表示列满秩的未知输入干扰的系数矩阵;wk-1表示系统噪声向量,Γk-1表示系统噪声分配矩阵,vk量测噪声向量;
系统状态的选取如下所示:
Xk=[φT (δvn)T (δp)T (εb)T (▽b)T]T
系统量测的选取如下所示:
根据惯性测量单元的误差分析和SINS机械编排可以得到忽略高阶小量后的SINS姿态误差方程、速度误差方程和位置误差方程(组)分别如下:
其中,为SINS姿态误差方程,为SINS速度误差方程,为SINS位置误差方程,φ为3维姿态失准角误差,εb为3维陀螺随机常值漂移,δvn表示3维速度误差,δp表示3维位置误差,表示3维加速度计随机常值漂移;
根据(1)-(3)组成的误差模型可以得到:
将F(t)离散化可得:
Fk|k-1≈I15×15+F(t)×Δt
其中Fk|k-1表示SINS的误差一步转移矩阵,Δt为SINS采样周期,I15×15表示15维单位矩阵。
上述的一种基于未知输入观测器的组合导航系统鲁棒抗扰滤波方法,所述步骤2具体方法为:
根据量测矩阵Hk将dk-1分解如下:
上述的一种基于未知输入观测器的组合导航系统鲁棒抗扰滤波方法,所述步骤3的具体方法为:
wi=α(1-α)i,i=0,1,…l-2,l-1 (6)
根据式(4)-(6)及对应窗口中的加权数据的方程,当某一维存在未知输入干扰时,其对应的均值必然会在上一时刻均值邻域之外,若对应的均值在邻域范围之内,则不存在未知输入干扰,邻域范围表示如下:
上述的一种基于未知输入观测器的组合导航系统鲁棒抗扰滤波方法,所述步骤4具体方法为:
量测信息到来时,卡尔曼滤波量测更新之前,对系统进行如下修正:
其中,表示第7~9维系统状态量的一步预测值;Nk表示量测中列满秩未知输入干扰的系数矩阵;Zk表示系统量测;表示含有未知输入干扰的第7~9维系统状态量的一步预测值,表示含未知输入的一步量测值,Mk-1表示列满秩的未知输入干扰的系数矩阵;表示可被直接观测的状态中的未知输入的估计值;表示量测中未知输入干扰向量的估计值
其中,当dk-1=0而ek≠0时,
当dk-1≠0且ek≠0时,
量测信息未到来时,卡尔曼滤波时间更新完成后,对系统进行如下修正:
本发明的有益效果是,相比传统滤波融合算法和传统鲁棒算法,本发明提出的滤波方法能够对突变型、斜坡型、交叠型、不同特征随机噪声等类型的未知输入干扰进行准确估计,具有更好的滤波估计效果和更强的鲁棒抗扰性,能够提高组合导航系统针对未知输入干扰的抗扰能力。
附图说明
下面结合附图和实施例对本发明进一步说明。
图1为仿真实验设计的仿真轨迹;
图2为在不含有未知输入干扰时正常滤波的卡尔曼滤波、基于Sage-Husa自适应滤波的自适应卡尔曼滤波和本发明方法的滤波结果中的定位误差曲线图;
图3为含有未知输入干扰时的卡尔曼滤波、基于Sage-Husa自适应滤波的自适应卡尔曼滤波和本发明方法的滤波结果中的定位误差曲线图;
图4为本发明方法中利用新息得到的纬度和经度未知输入干扰估计曲线;
图5为本发明方法中利用自校准技术得到的姿态角未知输入干扰估计曲线;
图6为本发明方法中利用自校准技术得到速度未知输入干扰估计曲线;
图7为采用车载实验数据的含有未知输入干扰时的卡尔曼滤波、基于Sage-Husa自适应滤波的自适应卡尔曼滤波和本发明方法的滤波结果中的定位误差曲线图;
图8为采用车载实验数据时本发明方法中利用新息得到的纬度和经度未知输入干扰估计曲线;
图9为采用车载实验数据时本发明方法中利用自校准技术得到的姿态角未知输入干扰估计曲线;
图10为采用车载实验数据时本发明方法中利用自校准技术得到的速度未知输入干扰估计曲线;
图11为卡尔曼滤波、基于Sage-Husa自适应滤波的自适应卡尔曼滤波和本发明方法滤波解算轨迹与GPS参考轨迹对比图。
具体实施方式
为使本领域技术人员更好的理解本发明的技术方案,下面结合附图和具体实施方式对本发明作详细说明。
首先,在步骤一中考虑未知输入dk-1∈Rp和ek∈Rm的存在,建立SINS/GNSS组合导航系统的滤波模型,模型形式为状态误差滤波模型。
Xk=Fk|k-1Xk-1+Mk-1dk-1+Γk-1wk-1
Zk=HkXk+Nkek+vk
式中Xk∈Rn表示系统的状态,Zk∈Rm表示系统量测,dk-1∈Rp和ek∈Rm表示未知输入干扰向量;Fk|k-1∈Rn×n表示SINS的误差一步转移矩阵,由SINS的各误差方程(组)系数决定,Hk∈Rm×n为系统量测矩阵;Mk-1∈Rn×p和Nk∈Rm×m表示列满秩的未知输入干扰的系数矩阵;wk-1∈Rs表示系统噪声向量,Γk-1∈Rn×s表示系统噪声分配矩阵,vk∈Rm表示量测噪声向量。
系统状态量的选取如下所示:
系统量测量的选取如下所示:
根据惯性测量单元的误差分析和SINS机械编排可以得到忽略高阶小量后的SINS姿态误差方程、速度误差方程和位置误差方程(组)分别如下
上述3式中,φ为失准角误差,表示导航坐标系相对惯性坐标系的角速度在导航坐标系的投影,表示角速度计算误差,为陀螺测量误差,εb为陀螺常值漂移,表示导航坐标系下的比力值,为地球自转角速度计算误差,为导航系旋转角速度计算误差。
根据上述3式构成的误差模型可以得到:
将上式离散化可得
Fk|k-1≈I15×15+F(t)×Δt
其中Δt为SINS采样周期。
至此,结合前文步骤一中的状态方程和量测方程以及各自的选取值情况,则同时在量测和状态中含有不同种类未知输入的SINS/GPS组合导航系统状态误差滤波模型建立完成。
rank(HkMk-1)=rank(Mk-1)=p
且有rank(Nk)=m,因此在组合导航系统中需要根据量测矩阵Hk将dk-1分解为可观和不可观的两种未知输入。
首先假设未知输入dk-1和ek不会同时存在,并考虑p>m且ek=0的情况,根据Hk将dk-1分解如下:
进一步地,考虑未知输入同时存在于状态和量测中的情况,根据指数加权滑动窗估计值、历史均值与方差判断未知输入存在位置。
本方法模型状态和量测中的导航信息均为误差量,理想情况下应为极小值,且为平稳过程,服从正态分布,故考虑利用指数加权滑动窗求得窗口内某一维的均值如下:
wi=α(1-α)i,i=0,1,…l-2,l-1
根据式上述3式及对应窗口中加权数据的方差,当某一维存在干扰时,其对应的均值必然会在上一时刻均值邻域之外,邻域范围表示如下:
式中σ为上述方差的标准差;cU为范围系数,取值与量测信号质量呈正相关。又模型中包含的是导航信息的误差量,所以理论上不会受到载体机动的影响,故可以根据是否满足在上式所示范围内来判断未知输入干扰的存在位置——若不满足则该维信息存在未知输入干扰,否则就不存在未知输入。
进一步地,需要用新息和状态自校准技术得到未知输入估计器并对系统状态和量测进行修正。
根据分解结果以及上述充分条件的变形式,令
并与前式相乘可得
考虑dk-1=0而ek≠0的情况,此时由信息可得未知输入干扰项变为
Nkek=Zk-Hk(Fk|k-1Xk-1+Γk-1wk-1)-vk
由rank(Nk)=m经简单推导可得:
当量测信息到来时,基于本发明步骤一种所建立的滤波模型,由上述各式可得到对未知输入干扰较为准确的估计值,基于此可对系统进行如下修正:
对于量测没有包含的未知输入,无法从新息中解出其估计值,需要从系统状态本身解耦出其估计值。根据所建系统模型,可知当k时刻开始存在未知输入干扰时有一步预测如下:
由上式可得到未知输入干扰项的估计值如等式左侧,进一步对状态进行修正即可抑制未知输入干扰的影响:
上式所示修正将在卡尔曼滤波时间更新后执行。由上述各式可知当系数矩阵Mk-1为对角阵且其中元素均取1时可得到dk-1的准确估计值,否则无法得到干扰的准确估计。当量测新息到来时,将利用自校准方法所得的未知输入估计值中对应维度替换为利用新息所得的估计值,并重记为即为最终估计结果。
当dk-1≠0且ek≠0时,新息中做差的量测和状态一步预测均有干扰,表示如下:
根据上述新息公式有:
将上述2个调整后的未知输入干扰估计值代入前文中量测新息到来时的状态和量测修正式即可实现对系统的修正。
为了验证本发明的有效性,通过设计对应的SINS/GPS松组合导航系统的仿真实验以及利用车载实验数据从系统抗扰种类、未知干扰估计结果以及组合导航结果误差等方面验证本发明在理论与实际应用中的有效性。
仿真条件设置如下:
为了使仿真实验贴近实际情况,设计的仿真轨迹包括加速、减速、匀速、直线、转弯以及转圈等部分,共持续672秒,轨迹如图1所示。设置陀螺零偏为随机游走为0.001°/h;加速度计零偏为100μg、随机游走为初始俯仰、横滚和航向失准角分别为0.5'、-0.5'和20',速度误差均为0.1m/s,位置误差均为3m;系统噪声标准差和量测噪声标准差参考上述各参数设置;SINS采用2子样误差补偿方法解算,计算输出频率为50Hz,GPS接收机输出频率为1Hz。由于仿真为相对理想的情况,故设置cU=1。为方便验证算法,仿真中设置Mk-1与Nk对角线元素均为1,其余元素为0,且列满秩。
由图2可见,在不含未知输入干扰时,卡尔曼滤波(KF)、基于Sage-Husa自适应滤波的自适应卡尔曼滤波(下文中简称AKF)与本发明(下文中简称SRKFUIE)的估计结果和误差精度差别不大,其中本发明在对状态和量测进行修正时会在未知输入估计及系统修正处引入极小误差导致滤波误差较前两者稍大,但在可接受范围内。
对于同样的仿真轨迹参数,注入未知输入干扰后采用3种算法进行组合导航滤波,结果如图3所示。由图3可知KF由于没有鲁棒性,在250s处加入多种干扰后滤波误差瞬间增大,滤波发散。AKF虽然通过自适应调整Rk和Qk使得对于噪声中的干扰具有一定的鲁棒性,但表1所示多种干扰还包括野值等非噪声类型。由于AKF自适应是基于新息的,状态和量测同时存在误差会导致新息失去反应误差干扰的能力,这会导致AKF的自适应Rk和Qk引入误差,因此AKF的滤波误差会显著大于KF。本发明SRKFUIE对状态中和量测新息中的干扰均有估计和相应的快速修正处理,但基于新息的未知输入干扰估计和处理会慢1拍,因此图3中本发明SRKFUIE会在有未知输入干扰的时段出现定位误差的波动,但误差值大多在±1m以内,可以接受。
本方法对于未知输入干扰的估计结果如图4~图6所示,其中dei,i=1,2对应利用新息得到的未知输入干扰的估计结果,当dk-1≠0且ek≠0时表示状态中未知输入干扰与量测中未知输入干扰二者估计之和;dki,i=1,2,…,8表示在系统状态中存在的未知输入干扰,eki,i=1,2,3表示在系统量测中存在的未知干扰,二者对应于预先的干扰设置;dexi,i=1,2,…,6对应利用自校准得到的未知输入干扰的估计结果,dezi,i=1,2,3对应式两种未知输入同时存在时的量测未知输入估计结果。通过图4~图6的结果,与预设未知干扰参数进行对比,可见未知输入干扰估计结果较为准确。由上述各图可以看出本发明在系统存在未知输入干扰时仍具有较好的滤波结果,能够实现对未知输入干扰的估计和抑制。
为了检验本发明对真实环境下导航数据的有效性,设计并进行了车载实验,并对车载实验数据进行半实物实验验证。实验设备包括汽车,GPS接收机、高精度IMU以及上位机,实验地点位于辽宁鞍山。实验中参数参考实验仪器参数设置,如下所示:IMU参数设置与仿真实验中相同;GPS东向、北向定位误差设置为2m,高度定位误差设置为3m;系统噪声标准差和量测噪声标准差参考上述各参数设置。SINS解算频率与GPS输出频率与仿真实验中设置均相同。由于实验环境较为复杂,故设置cU=3。Mk-1与Nk的设置与仿真实验相同。在本实验中,采用精度较高的GPS数据作为参考数据计算不同组合导航滤波算法的定位误差。
图7中在200s附近东向定位误差出现剧烈波动,该误差由GPS经度信号波动引起。此时系统中尚未注入未知输入干扰,属于原始数据自身耦合的小数值未知干扰,AKF与KF对其都具有一定程度的抑制,但作用并不明显。本发明SRKFUIE能够检测出该处干扰(于图8经度未知输入估计中有所体现)大小及存在位置,并及时对系统进行修正,使得定位误差绝对值小于AKF和KF,从另一方面验证了本发明的有效性。
图8~图10中各图例符号含义与仿真实验中相同,可见本发明应用于实验数据中也能较准确的得到未知输入的估计。200s处由于GPS经度信号出现剧烈波动被本算法检测到并作为了未知输入进行处理,因此图8对应时间处出现了估计值的波动,这一现象符合实际情况,是正常的。图11显示出3种算法滤波解算轨迹与GPS轨迹对比情况,可以看出在没有未知输入干扰时3那种算法轨迹几乎重合,当干扰到来后KF和AKF结果马上出现偏移,本发明结果稍有偏差(<5m)后快速恢复正常。
综上,相比传统滤波融合算法和传统鲁棒算法,本发明提出的滤波方法能够对突变型、斜坡型、交叠型、不同特征随机噪声等类型的未知输入干扰进行准确估计,具有更好的滤波估计效果和更强的鲁棒抗扰性。
以上实施例仅为本发明的示例性实施例,不用于限制本发明,本发明的保护范围由权利要求书限定。本领域技术人员可以在本发明的实质和保护范围内,对本发明做出各种修改或等同替换,这种修改或等同替换也应视为落在本发明的保护范围内。
Claims (5)
1.基于未知输入观测器的组合导航系统鲁棒抗扰滤波方法,其特征在于,包括以下步骤:
步骤一:考虑未知输入dk-1∈Rp和ek∈Rm的存在,建立SINS/GNSS组合导航系统的滤波模型;
步骤二:根据量测矩阵Hk将dk-1分解为可观和不可观的两种未知输入;
步骤三:根据指数加权滑动窗估计值、历史均值与方差判断未知输入存在位置;
步骤四:利用新息和状态自校准技术设计出未知输入估计器并对系统状态和量测进行修正。
2.根据权利要求1所述的一种基于未知输入观测器的组合导航系统鲁棒抗扰滤波方法,其特征在于,所述步骤1具体方法为:
Xk=Fk|k-1Xk-1+Mk-1dk-1+Γk-1wk-1
Zk=HkXk+Nkek+vk
式中Xk表示系统的状态,Zk表示系统量测,dk-1和ek表示未知输入干扰向量;Fk|k-1表示SINS的误差一步转移矩阵,由SINS的各误差方程(组)系数决定;Hk为系统量测矩阵;Mk-1和Nk表示列满秩的未知输入干扰的系数矩阵;wk-1表示系统噪声向量,Γk-1表示系统噪声分配矩阵,vk量测噪声向量;
系统状态的选取如下所示:
Xk=[φT (δvn)T (δp)T (εb)T (▽b)T]T
其中,φ表示3维姿态失准角误差,δvn表示3维速度误差,δp表示3维位置误差,εb表示3维陀螺随机常值漂移,▽b表示3维加速度计随机常值漂移,共15维状态量;
系统量测的选取如下所示:
根据惯性测量单元的误差分析和SINS机械编排可以得到忽略高阶小量后的SINS姿态误差方程、速度误差方程和位置误差方程(组)分别如下:
其中,为SINS姿态误差方程,为SINS速度误差方程,为SINS位置误差方程,φ为3维姿态失准角误差,εb为3维陀螺随机常值漂移,δvn表示3维速度误差,δp表示3维位置误差,表示3维加速度计随机常值漂移;
根据(1)-(3)组成的误差模型可以得到:
将F(t)离散化可得:
Fk|k-1≈I15×15+F(t)×Δt
其中Fk|k-1表示SINS的误差一步转移矩阵,Δt为SINS采样周期,I15×15表示15维单位矩阵。
4.根据权利要求1所述的一种基于未知输入观测器的组合导航系统鲁棒抗扰滤波方法,其特征在于,所述步骤3的具体方法为:
wi=α(1-α)i,i=0,1,…l-2,l-1 (6)
根据式(4)-(6)及对应窗口中的加权数据的方程,当某一维存在未知输入干扰时,其对应的均值必然会在上一时刻均值邻域之外,若对应的均值在邻域范围之内,则不存在未知输入干扰,邻域范围表示如下:
5.根据权利要求1所述的一种基于未知输入观测器的组合导航系统鲁棒抗扰滤波方法,其特征在于,所述步骤4具体方法为:
量测信息到来时,卡尔曼滤波量测更新之前,对系统进行如下修正:
其中,表示第7~9维系统状态量的一步预测值;Nk表示量测中列满秩未知输入干扰的系数矩阵;Zk表示系统量测;表示含有未知输入干扰的第7~9维系统状态量的一步预测值,表示含未知输入的一步量测值,Mk-1表示列满秩的未知输入干扰的系数矩阵;表示可被直接观测的状态中的未知输入的估计值;表示量测中未知输入干扰向量的估计值
其中,当dk-1=0而ek≠0时,
当dk-1≠0且ek≠0时,
量测信息未到来时,卡尔曼滤波时间更新完成后,对系统进行如下修正:
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202210748607.2A CN115265528A (zh) | 2022-06-29 | 2022-06-29 | 基于未知输入观测器的组合导航系统鲁棒抗扰滤波方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202210748607.2A CN115265528A (zh) | 2022-06-29 | 2022-06-29 | 基于未知输入观测器的组合导航系统鲁棒抗扰滤波方法 |
Publications (1)
Publication Number | Publication Date |
---|---|
CN115265528A true CN115265528A (zh) | 2022-11-01 |
Family
ID=83763634
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202210748607.2A Pending CN115265528A (zh) | 2022-06-29 | 2022-06-29 | 基于未知输入观测器的组合导航系统鲁棒抗扰滤波方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN115265528A (zh) |
Citations (8)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US20160370467A1 (en) * | 2015-06-19 | 2016-12-22 | Profound Positioning Inc. | Methods and systems for performing global navigation satellite system (gnss) orbit and clock augmentation and position determination |
CN106885570A (zh) * | 2017-02-24 | 2017-06-23 | 南京理工大学 | 一种基于鲁棒sckf滤波的紧组合导航方法 |
CN108594272A (zh) * | 2018-08-01 | 2018-09-28 | 北京航空航天大学 | 一种基于鲁棒卡尔曼滤波的抗欺骗干扰组合导航方法 |
CN108594271A (zh) * | 2018-08-01 | 2018-09-28 | 北京航空航天大学 | 一种基于复合分层滤波的抗欺骗干扰的组合导航方法 |
CN110008638A (zh) * | 2019-04-23 | 2019-07-12 | 河海大学 | 一种基于自适应EnKF技术的动态状态估计方法 |
CN110567455A (zh) * | 2019-09-25 | 2019-12-13 | 哈尔滨工程大学 | 一种求积更新容积卡尔曼滤波的紧组合导航方法 |
CN110677140A (zh) * | 2019-10-28 | 2020-01-10 | 北京航空航天大学 | 一种含未知输入和非高斯量测噪声的随机系统滤波器 |
CN113916226A (zh) * | 2021-10-09 | 2022-01-11 | 哈尔滨工业大学 | 一种基于最小方差的组合导航系统抗扰滤波方法 |
-
2022
- 2022-06-29 CN CN202210748607.2A patent/CN115265528A/zh active Pending
Patent Citations (8)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US20160370467A1 (en) * | 2015-06-19 | 2016-12-22 | Profound Positioning Inc. | Methods and systems for performing global navigation satellite system (gnss) orbit and clock augmentation and position determination |
CN106885570A (zh) * | 2017-02-24 | 2017-06-23 | 南京理工大学 | 一种基于鲁棒sckf滤波的紧组合导航方法 |
CN108594272A (zh) * | 2018-08-01 | 2018-09-28 | 北京航空航天大学 | 一种基于鲁棒卡尔曼滤波的抗欺骗干扰组合导航方法 |
CN108594271A (zh) * | 2018-08-01 | 2018-09-28 | 北京航空航天大学 | 一种基于复合分层滤波的抗欺骗干扰的组合导航方法 |
CN110008638A (zh) * | 2019-04-23 | 2019-07-12 | 河海大学 | 一种基于自适应EnKF技术的动态状态估计方法 |
CN110567455A (zh) * | 2019-09-25 | 2019-12-13 | 哈尔滨工程大学 | 一种求积更新容积卡尔曼滤波的紧组合导航方法 |
CN110677140A (zh) * | 2019-10-28 | 2020-01-10 | 北京航空航天大学 | 一种含未知输入和非高斯量测噪声的随机系统滤波器 |
CN113916226A (zh) * | 2021-10-09 | 2022-01-11 | 哈尔滨工业大学 | 一种基于最小方差的组合导航系统抗扰滤波方法 |
Non-Patent Citations (2)
Title |
---|
TRISTAN D 等: "Adaptive_Estimation_of_Unknown_Inputs_with_Weakly_Nonlinear_Dynamics", 《2022 AMERICAN CONTROL CONFERENCE (ACC) ATLANTA》, 10 June 2022 (2022-06-10), pages 5043 - 5049, XP034179528, DOI: 10.23919/ACC53348.2022.9867862 * |
尹冉冉 等: "SINS_GNSS组合导航技术研究", 《导航天地》, 30 June 2012 (2012-06-30), pages 66 - 70 * |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN109459019B (zh) | 一种基于级联自适应鲁棒联邦滤波的车载导航计算方法 | |
CN111156994B (zh) | 一种基于mems惯性组件的ins/dr&gnss松组合导航方法 | |
CN112013836B (zh) | 一种基于改进自适应卡尔曼滤波的航姿参考系统算法 | |
CN110702104B (zh) | 一种基于车辆零速检测的惯性导航误差修正方法 | |
US7643939B2 (en) | Methods and systems for implementing an iterated extended Kalman filter within a navigation system | |
CN110221332B (zh) | 一种车载gnss/ins组合导航的动态杆臂误差估计和补偿方法 | |
EP1200802B1 (en) | Vibration compensation for sensors | |
CN110017837B (zh) | 一种姿态抗磁干扰的组合导航方法 | |
US7058505B1 (en) | System for navigation redundancy | |
US20070100550A1 (en) | Systems and methods for reducing vibration-induced errors in inertial sensors | |
CN111337020A (zh) | 引入抗差估计的因子图融合定位方法 | |
CN112505737B (zh) | 一种gnss/ins组合导航方法 | |
CN109945859B (zh) | 一种自适应h∞滤波的运动学约束捷联惯性导航方法 | |
US20190277655A1 (en) | Self-calibrating inertial measurement system and method | |
CN112945225A (zh) | 基于扩展卡尔曼滤波的姿态解算系统及解算方法 | |
Cao et al. | Anti-disturbance fault tolerant initial alignment for inertial navigation system subjected to multiple disturbances | |
CN116007620A (zh) | 一种组合导航滤波方法、系统、电子设备及存储介质 | |
CN114526731A (zh) | 一种基于助力车的惯性组合导航方向定位方法 | |
CN110595434B (zh) | 基于mems传感器的四元数融合姿态估计方法 | |
CN112304337A (zh) | 基于陀螺仪和加速度计的运动角度估计方法和系统 | |
CN116399351A (zh) | 一种车辆位置估计方法 | |
CN115265528A (zh) | 基于未知输入观测器的组合导航系统鲁棒抗扰滤波方法 | |
CN110736459A (zh) | 惯性量匹配对准的角形变测量误差评估方法 | |
CN114264304B (zh) | 复杂动态环境高精度水平姿态测量方法与系统 | |
JP3367461B2 (ja) | 移動体姿勢角検出装置 |
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 |