CN116150565A - 一种用于单频gnss/mems-imu/里程计低功耗实时组合导航的改进自适应抗差卡尔曼滤波算法及系统 - Google Patents
一种用于单频gnss/mems-imu/里程计低功耗实时组合导航的改进自适应抗差卡尔曼滤波算法及系统 Download PDFInfo
- Publication number
- CN116150565A CN116150565A CN202310340961.6A CN202310340961A CN116150565A CN 116150565 A CN116150565 A CN 116150565A CN 202310340961 A CN202310340961 A CN 202310340961A CN 116150565 A CN116150565 A CN 116150565A
- Authority
- CN
- China
- Prior art keywords
- state
- kalman filtering
- matrix
- prediction
- gnss
- 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 105
- 238000004422 calculation algorithm Methods 0.000 title claims abstract description 51
- 239000011159 matrix material Substances 0.000 claims abstract description 127
- 239000013598 vector Substances 0.000 claims abstract description 69
- 230000003044 adaptive effect Effects 0.000 claims abstract description 59
- 238000005259 measurement Methods 0.000 claims abstract description 17
- 238000000034 method Methods 0.000 claims description 46
- 230000007704 transition Effects 0.000 claims description 35
- 230000008569 process Effects 0.000 claims description 22
- 238000007667 floating Methods 0.000 claims description 15
- 230000008859 change Effects 0.000 claims description 5
- 238000000354 decomposition reaction Methods 0.000 claims description 5
- 238000013461 design Methods 0.000 claims description 5
- 230000002159 abnormal effect Effects 0.000 abstract description 11
- 235000002020 sage Nutrition 0.000 description 14
- 238000012360 testing method Methods 0.000 description 12
- 102000027426 receptor tyrosine kinases Human genes 0.000 description 11
- 108091008598 receptor tyrosine kinases Proteins 0.000 description 11
- 238000004364 calculation method Methods 0.000 description 8
- 230000000694 effects Effects 0.000 description 7
- 230000006870 function Effects 0.000 description 4
- 238000013459 approach Methods 0.000 description 3
- 230000005856 abnormality Effects 0.000 description 2
- 238000007792 addition Methods 0.000 description 2
- 238000004458 analytical method Methods 0.000 description 2
- 230000000295 complement effect Effects 0.000 description 2
- 238000010276 construction Methods 0.000 description 2
- 238000009795 derivation Methods 0.000 description 2
- 238000011026 diafiltration Methods 0.000 description 2
- 238000002474 experimental method Methods 0.000 description 2
- 230000004927 fusion Effects 0.000 description 2
- 238000006467 substitution reaction Methods 0.000 description 2
- 238000012795 verification Methods 0.000 description 2
- 230000009471 action Effects 0.000 description 1
- 230000006978 adaptation Effects 0.000 description 1
- 238000012937 correction Methods 0.000 description 1
- 238000005516 engineering process Methods 0.000 description 1
- 230000007613 environmental effect Effects 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 230000004044 response Effects 0.000 description 1
- 238000005070 sampling Methods 0.000 description 1
- 238000000926 separation method Methods 0.000 description 1
- 230000003595 spectral effect Effects 0.000 description 1
- 230000009466 transformation Effects 0.000 description 1
Images
Classifications
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06F—ELECTRIC DIGITAL DATA PROCESSING
- G06F17/00—Digital computing or data processing equipment or methods, specially adapted for specific functions
- G06F17/10—Complex mathematical operations
- G06F17/16—Matrix or vector computation, e.g. matrix-matrix or matrix-vector multiplication, matrix factorization
-
- 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
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06F—ELECTRIC DIGITAL DATA PROCESSING
- G06F17/00—Digital computing or data processing equipment or methods, specially adapted for specific functions
- G06F17/10—Complex mathematical operations
- G06F17/17—Function evaluation by approximation methods, e.g. inter- or extrapolation, smoothing, least mean square method
Landscapes
- Engineering & Computer Science (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Remote Sensing (AREA)
- Radar, Positioning & Navigation (AREA)
- Mathematical Physics (AREA)
- Pure & Applied Mathematics (AREA)
- Mathematical Optimization (AREA)
- Mathematical Analysis (AREA)
- Data Mining & Analysis (AREA)
- Theoretical Computer Science (AREA)
- Computational Mathematics (AREA)
- Algebra (AREA)
- Databases & Information Systems (AREA)
- Software Systems (AREA)
- General Engineering & Computer Science (AREA)
- Automation & Control Theory (AREA)
- Computing Systems (AREA)
- Navigation (AREA)
Abstract
本发明涉及一种用于单频GNSS/MEMS‑IMU/里程计低功耗实时组合导航的改进自适应抗差卡尔曼滤波算法及系统，这种方法能够有效地削弱测量中异常模型偏差和异常值的影响，提高了具有脆弱性又不输出观测向量协方差的单频GNSS的组合导航系统的定位精度，满足城市复杂环境下低功耗实时嵌入式车载应用。本算法通过预测残差得到尺度因子，并利用尺度因子来调整在不同GNSS解状态下人为设定的观测向量协方差矩阵，使观测向量的协方差矩阵连续的随着复杂场景而变化。然后再用马氏距离计算出自适应因子，利用自适应因子缩放状态预测协方差矩阵。最后结合一步预测卡尔曼滤波算法来减少计算复杂度。
Description
技术领域
本发明涉及一种应用于单频GNSS/MEMS-IMU/里程计低功耗实时组合导航的改进自适应抗差卡尔曼滤波算法及系统。
背景技术
全球定位导航卫星系统(Global Navigation Satellite System,GNSS)具有较高的定位精度，没有累积误差。但GNSS接收机至少需要4颗卫星才能定位。在林荫道、高架桥、城市峡谷和隧道等环境下，卫星信号易受到遮挡和多径影响。采用载波相位差分算法的GNSS接收机在复杂环境下容易出现失锁现象，并且重新再进入开阔环境时需要数秒时间才能固定整周模糊度。这会导致GNSS的定位精度变差，定位的连续性和可用性也不能得到保证。惯性导航具有较高的短时定位精度，不受环境限制。但由于惯性测量单元存在器件噪声、零偏、比例因子、轴偏差、非线性度等误差，单纯的INS定位精度会随时间呈指数函数发散。取长补短，将GNSS和INS组合使用，可以提供更精确、连续和可靠的导航信息(包括位置、速度和姿态)。换句话说，通过多源异构传感器的信息融合，可实现优势互补的作用。
卡尔曼滤波是组合导航最常用的最优估计方法之一。但是它的性能会被GNSS量测异常值或者非高斯分布的噪声退化。针对这一问题，近年来人们提出了各种滤波算法来控制其对组合导航定位精度的影响，例如，抗差卡尔曼滤波，H滤波，DIA滤波，抗差间隔卡尔曼滤波，基于马氏距离的自适应抗差滤波和自适应卡尔曼滤波等。抗差滤波可以控制测量异常值对运动状态估计的影响，但不能有效地控制模型偏差的影响。H滤波可以解决随机模型的干扰，但对GNSS异常值无能为力。此外该方法的计算量较大。DIA滤波即detection,identification,and adaptation，该方法可以消除异常值对组合导航的影响，但是他在量测不可靠的时候对异常值的识别特别困难。抗差间隔卡尔曼滤波是将抗差估计与间隔卡尔曼滤波相结合，它可以处理系统模型参数不确定性，减小模型误差的影响。基于马氏距离的自适应抗差滤波是基于马氏距离定义判断指标，当观测值可靠时执行自适应卡尔曼滤波，否则采用抗差估计方法进行抗差滤波。该方法可以弱化测量中异常模型偏差和异常值的影响。自适应卡尔曼滤波可以控制模型误差的影响，然而，基于预测残差的自适应卡尔曼滤波需要可靠的观测信息，其性能受到测量中异常值的显著影响。不同于预测残差策略的自适应滤波，Yang et al.提出了将抗差估计和自适应因子相结合的adaptive robust filtertechnology,该技术能有效地抑制观测异常和载体状态扰动异常对动态系统参数估计值的影响。
以上这些方法解决了在动态条件下GNSS观测异常和模型偏差而导致组合导航定位精度差的问题。然而，它们对存在脆弱性又不输出观测向量协方差的单频GNSS的作用是有限的。此外，上述方法无法运行在低功耗嵌入式芯片中。
发明内容
发明目的：为了提高低成本组合导航定位模块在复杂场景下的定位精度和降低功耗，使其能够广泛应用于车辆导航领域，本发明提供一种应用于单频GNSS/MEMS-IMU/里程计低功耗实时组合导航的改进自适应抗差卡尔曼滤波算法。
技术方案：为实现上述目的，本发明采用的技术方案为：首先，利用GNSS观测信息计算预测残差，根据预测残差得到估计的观测向量协方差矩阵。然后将其和人工设定的观测向量协方差矩阵进行比较。如果估计的观测向量协方差矩阵大于设定的观测向量协方差矩阵，则此时存在异常GNSS。随后，比例因子调整观测向量的集合协方差矩阵，以减少卡尔曼滤波器中GNSS的权重。相反，当前GNSS观测质量与估计值一致，无需调整观测向量协方差矩阵R。对于异常观测值，由比例因子计算的观测向量协方差矩阵的估计值变得不可预测。引入基于预测残差的马氏距离来检测扰动异常，并获得自适应因子来膨胀状态预测协方差矩阵。
此外，还考虑了一个特殊情况。GNSS RTK易受干扰，车辆进入隧道时会变成伪固定解，参与组合导航后会干扰系统状态，导致隧道内定位精度差。PDOP值和跟踪的卫星数量反映了天气和环境对GNSS定位精度的影响。利用这两个参数，可以识别并消除全球导航卫星系统的伪固定解。最后，为了降低组合导航算法的计算复杂度，采用了一步预测卡尔曼滤波器。在一步预测卡尔曼滤波器中，卡尔曼预测过程仅计算状态转移矩阵，预测状态协方差矩阵在卡尔曼滤波器更新过程中执行。这样就减少了状态估计向量协方差矩阵P的计算量。通过减少算法的计算量，降低了模块中MCU的时钟频率，实现了模块的低功耗。应用于GNSS/INS/里程计组合导航模块的改进鲁棒自适应卡尔曼滤波算法的算法流程于附图1中详细描述。
具体采用如下技术方案：
一种用于单频GNSS/MEMS-IMU/里程计低功耗实时组合导航的改进自适应抗差卡尔曼滤波算法，其特征在于：所述卡尔曼滤波预测及更新包括
卡尔曼滤波预测步骤：根据系统的模型，基于系统的上一状态而预测出现在状态，如公式(1)，再利用上一时刻的状态转移矩阵与当前时刻的状态转移矩阵相乘得到当前时刻状态转移矩阵的估计值；预测过程状态中先计算误差状态向量X，再计算状态转移矩阵Φ，公式如下：
Xk,k-1＝Φk,k-1Xk (1)
卡尔曼滤波更新步骤：根据预测步骤中的公式得到状态预测结果后，再利用量测值和预测值对当前状态进行最优估计，得到最优的状态估计值；更新过程中首先计算状态预测协方差矩阵P，然后分别计算比例因子和自适应因子，再将比例因子和自适应因子代入计算增益K的公式中，最后利用增益矩阵更新误差状态向量X和状态估计协方差矩阵P。
在上述的一种用于单频GNSS/MEMS-IMU/里程计低功耗实时组合导航的改进自适应抗差卡尔曼滤波算法，卡尔曼滤波更新步骤中，采用如下公式
Xk＝Xk,k-1+Kk(Zk-HkXk,k-1) (7)
Pk＝(I-KkHk)Pk,k-1 (8)
其中，X是误差状态向量；Φ表示状态转移矩阵；P是状态预测协方差矩阵；Q是系统噪声协方差矩阵；K是卡尔曼滤波增益矩阵，它决定观测信息的权重；H表示设计矩阵；R代表观测噪声协方差矩阵；Z代表观测向量；α是比例因子；β是自适应因子；trace()表示对矩阵求迹；
表示卡方分布；/>
是预测残差向量；以上的下角标代表状态的变化，例如，Xk,k-1表示从k-1历元到k历元的误差状态向量。
在上述的一种用于单频GNSS/MEMS-IMU/里程计低功耗实时组合导航的改进自适应抗差卡尔曼滤波算法，当GNSS RTK为固定解时，计算比例因子根据以下公式
其中，
是固定解的观测向量协方差矩阵。
在上述的一种用于单频GNSS/MEMS-IMU/里程计低功耗实时组合导航的改进自适应抗差卡尔曼滤波算法，当GNSS RTK为浮点解时，计算比例因子根据以下公式
其中，
是浮点解的观测向量协方差矩阵。
在上述的一种用于单频GNSS/MEMS-IMU/里程计低功耗实时组合导航的改进自适应抗差卡尔曼滤波算法，当GNSS为伪距差分解时，计算比例因子根据以下公式
其中，
是浮点解的观测向量协方差矩阵。
在上述的一种用于单频GNSS/MEMS-IMU/里程计低功耗实时组合导航的改进自适应抗差卡尔曼滤波算法，计算自适应因子根据以下公式
βk(i)的初始值βk(0)＝1。
一种用于单频GNSS/MEMS-IMU/里程计低功耗实时组合导航的改进自适应抗差卡尔曼滤波系统，包括
卡尔曼滤波预测模块：根据系统的模型，基于系统的上一状态而预测出现在状态，如公式(1)，再利用上一时刻的状态转移矩阵与当前时刻的状态转移矩阵相乘得到当前时刻状态转移矩阵的估计值；预测过程状态中先计算误差状态向量X，再计算状态转移矩阵Φ，公式如下：
Xk,k-1＝Φk,k-1Xk (1)
卡尔曼滤波更新模块：根据预测步骤中的公式得到状态预测结果后，再利用量测值和预测值对当前状态进行最优估计，得到最优的状态估计值；更新过程中首先计算状态预测协方差矩阵P，然后分别计算比例因子和自适应因子，再将比例因子和自适应因子代入计算增益K的公式中，最后利用增益矩阵更新误差状态向量X和状态估计协方差矩阵P。
在上述的系统，卡尔曼滤波更新模块中，采用如下公式
Xk＝Xk,k-1+Kk(Zk-HkXk,k-1) (7)
Pk＝(I-KkHk)Pk,k-1 (8)
其中，X是误差状态向量；Φ表示状态转移矩阵；P是状态预测协方差矩阵；Q是系统噪声协方差矩阵；K是卡尔曼滤波增益矩阵，它决定观测信息的权重；H表示设计矩阵；R代表观测噪声协方差矩阵；Z代表观测向量；α是比例因子；β是自适应因子；trace()表示对矩阵求迹；
表示卡方分布；/>
是预测残差向量；以上的下角标代表状态的变化，例如，Xk,k-1表示从k-1历元到k历元的误差状态向量。
在上述的系统，当GNSS RTK为固定解时，计算比例因子根据以下公式
其中，
是固定解的观测向量协方差矩阵；
当GNSS RTK为浮点解时，计算比例因子根据以下公式
其中，
是浮点解的观测向量协方差矩阵；
当GNSS为伪距差分解时，计算比例因子根据以下公式
其中，
是浮点解的观测向量协方差矩阵。
在上述的系统，计算自适应因子根据以下公式
βk(i)的初始值βk(0)＝1。
本发明在组合导航的初始化过程中，采用了一种动态对准方法来校准INS，即使用卫星进行动态对准。在动态对准中，利用GNSS信息，通过杆臂矢量修正，可以获得IMU的位置和速度信息。由于车辆处于水平面上，可以认为俯仰和横摇为零。此时，IMU的姿态信息仍然比较粗糙。在后续过程中，姿态信息利用GNSS提供的位置信息通过卡尔曼滤波收敛，实现精对准。
附图说明
图1是改进自适应抗差卡尔曼滤波算法流程图。
图2是测试中标准扩展卡尔曼滤波和一步预测卡尔曼滤波各操作类型的运算次数。
图3是测试中标准扩展卡尔曼滤波和一步预测卡尔曼滤波在北、东、下方向的定位误差：(a)曲线重叠；(b)曲线分离。
图4是测试中传统自适应抗差卡尔曼滤波、Sage滤波、标准卡尔曼滤波、改进自适应抗差卡尔曼滤波和GNSS在林荫道场景中的定位误差。
图5是测试中传统自适应抗差卡尔曼滤波、Sage滤波、标准卡尔曼滤波和改进自适应抗差卡尔曼滤波的实时位置、速度、姿态误差。
图6是测试中改进自适应抗差卡尔曼滤波、传统自适应抗差卡尔曼滤波、标准卡尔曼滤波和Sage滤波的实时功耗。
具体实施方式：
下面通过实施例，并结合附图，对本发明的技术方案作进一步具体的说明。
实施例：
本实施例主要包括：
一、下面简述本发明的主要步骤，主要包括
卡尔曼滤波预测步骤：
根据系统的模型，基于系统的上一状态而预测出现在状态，如公式(4-19)，再利用上一时刻的状态转移矩阵与当前时刻的状态转移矩阵相乘得到当前时刻状态转移矩阵的估计值。预测过程状态中先计算误差状态向量X，再计算状态转移矩阵Φ，公式如下：
Xk,k-1＝Φk,k-1Xk (4-19)
卡尔曼滤波更新步骤：
根据预测步骤中的公式得到状态预测结果后，再利用量测值和预测值对当前状态进行最优估计，得到最优的状态估计值。更新过程中首先计算状态预测协方差矩阵P，然后分别计算比例因子和自适应因子，再将比例因子和自适应因子代入计算增益K的公式中，最后利用增益矩阵更新误差状态向量X和状态估计协方差矩阵P，公式如下：
Xk＝Xk,k-1+Kk(Zk-HkXk,k-1) (4-23)
Pk＝(I-KkHk)Pk,k-1 (4-24)
其中，X是误差状态向量；Φ表示状态转移矩阵；P是状态预测协方差矩阵；Q是系统噪声协方差矩阵；K是卡尔曼滤波增益矩阵，它决定观测信息的权重；H表示设计矩阵；R代表观测噪声协方差矩阵；Z代表观测向量；α是比例因子；β是自适应因子；trace()表示对矩阵求迹；
表示卡方分布；/>
是预测残差向量。以上的下角标代表状态的变化，例如，Xk,k-1表示从k-1历元到k历元的误差状态向量。
相比于传统卡尔曼滤波算法，本发明对卡尔曼滤波进行了改进，将状态预测向量协方差矩阵放到量测更新过程中，让其与量测更新一起执行。在预测过程中只计算状态向量和状态转移矩阵。为验证这种做法的可行性，对改进自适应抗差卡尔曼滤波算法的过程进行了详细推导，如下：
上式中的状态转移矩阵Φ展开后如下，
其中，
I3是三维单位矩阵，fn是n系下的比力，tgb和tab分别是陀螺和加速度计零偏相关时间。
在公式(4-21)中P阵的更新精度主要依赖于状态转移矩阵。以扩展卡尔曼滤波中k、k+1和k+2三个相邻历元的P矩阵预测为例，推导简化卡尔曼滤波中P矩阵的更新，过程如下：
历元k：
历元k+1：
历元k+2：
其中，Q是常谱密度矩阵，
是系统噪声协方差矩阵。
比较式(4-27)，(4-29)和(4-31)可以发现三个公式的结构特别相似。为了将预测状态协方差矩阵P放到量测更新中运行，需要利用状态转移矩阵来实现这一目标。因此需通过理论推理来证明该想法的可行性。公式(4-31)是从历元k到k+3的预测状态协方差矩阵，将其展开，
观察上式，该式等号右边第一项和第二项较容易计算，第三项和第四项相对难计算。如果要计算第三项和第四项则必须保存上一时刻Φk+2,k+1阵的值，但是随着时间的增长，保存的Φ矩阵数量也增多，这样严重浪费了硬件内存资源。因此为了方便计算，将第三项和第四项构造成第一项的形式，将其写成如下形式：
于是，将(4-33)和(4-34)式替换(4-32)式中的后两项可得，
为了验证(4-33)和(4-34)替换公式(4-32)中等号右边第三项和第四项的可行性，将(4-35)-(4-32)，如果两式相减的结果趋近于零，则表示构造方法可行。公式(4-35)-(4-32)展开后如下所示，
观察状态转移矩阵(4-25)式，可以将其假设成如下形式，
Φk≈I+FkΔt (4-37)其中采样时间间隔Δt＝0.01s。将假设的状态转移矩阵代入(4-36)式中，具体如下所示，
观察上式发现，结果是Δt的高阶小量趋近于零。因此，公式(4-31)中等号右边第三项和第四项可以被(4-33)和(4-34)代替，写成如下形式，
其中，Pk+3,k是从历元k到k+3的预测状态协方差矩阵。从上式可以看出P阵从历元k到k+3可以一步实现。从历元k到k+3的状态转移矩阵可以写成如下形式，
Φk+3,k＝Φk+3,k+2Φk+2,k+1Φk+1,k (4-40)
类似的，将P和Φ扩展到n个历元，即从k到k+n，如下，
Φk+n,k＝Φk+n,k+n-1Φk+n-1,k+n-2…Φk+2,k+1Φk+1,k (4-42)
因此，预测状态协方差矩阵可以在卡尔曼滤波更新过程中运行。同时，在卡尔曼滤波预测过程中，只需将状态转移矩阵每次计算得到的值乘以前一历元中计算的值便可得到当前历元更新的状态转移矩阵。在嵌入式芯片代码中运行的Φ和P可以表示成如下，
从以上推导过程可以看出，在卡尔曼滤波预测过程中只计算Φ阵所消耗的运算量远小于传统卡尔曼滤波算法中计算P阵所消耗的计算量。
二、卡尔曼滤波更新步骤中，两个因子的计算详细步骤如下：
计算比例因子：
根据协方差矩阵传播定律可知预测残差的理论协方差为：
根据硬件条件选择计算量小的单历元预测残差估计法来估计预测残差的协方差。通过
可求得/>
的协方差矩阵为：/>
由于短时间内动力学模型误差较小，通过比较预测残差的理论协方差和估计协方差可以看出量测值的误差情况。在自适应滤波时，如果观测向量误差较大，则量测向量的协方差矩阵Rk应变成αkRk(αk＞1)，意味着量测向量Zk的权重被弱化了。但是，我们希望经自适应因子作用后的预测残差协方差的理论值和估计值基本一致。因此，假设公式(5-9)和公式(5-10)相等，即：
根据上式可得量测向量协方差矩阵的估计值为：
将
与Rk取迹后，比例因子为
因此，当GNSS RTK为固定解时，比例因子为
其中，
是固定解的观测向量协方差矩阵。
当GNSS RTK为浮点解时，比例因子为
其中，
是浮点解的观测向量协方差矩阵。
当GNSS为伪距差分解时，比例因子为
其中，
是浮点解的观测向量协方差矩阵。
计算自适应因子：
随机系统状态空间模型
对于上式，观测向量服从均值为
方差为/>
的高斯分布，其概率密度函数ρ(Zk)为，/>
其中，m是观测向量的维数。如果存在粗差则上式的概率密度函数不成立。因此，可以根据这一特征来判断系统状态中是否存在异常值。
根据(5-19)式构造检验统计量来检测模型误差。因此，从观测值Zk到均值
的马氏距离平方被当作检验统计量，
其中，
是马氏距离。根据以上对马氏距离的定义可知，观测向量和状态预测向量之间的预测残差就是马氏距离，因此，预测残差的马氏距离平方为
预测残差服从标准正态分布，所以基于预测残差的马氏距离平方服从自由度为m的卡方分布
其中，α表示显著性水平，该值特别小，在这里我们将它设置为0.01；m是预测残差的维度；P{}表示γk大于
的概率，其概率值为α。然而，γk大于/>
是一个小概率事件，如果这个小概率事件成立，则表明系统状态中有异常值；否则，没有异常值。根据这一特性可以得到自适应因子，再利用自适应因子膨胀状态预测协方差矩阵，
于是，根据公式(5-22)假设下式成立
变换后如下
对上式利用牛顿-高斯插值法可得到自适应因子
其中，i表示第i次迭代；根据逆矩阵的倒数
和公式(5.25)可得f′(βk)如下/>
将(5-25)和(5-27)式代入(5-26)可得
βk(i)的初始值βk(0)＝1。
三、下面是采用本发明的具体案例。
(1)运算次数分析与比较
为了验证简化的一步预测卡尔曼滤波器在改进的抗差自适应算法中的有效性，对其计算量和定位性能进行了分析，并与标准的扩展卡尔曼滤波进行了比较。在改进的自适应抗差卡尔曼滤波(Improved robust adaptive Kalman filter，IRAKF)算法中，使用了简化的一步预测卡尔曼滤波。即预测状态协方差矩阵仅在测量更新周期中计算一次。因此，该算法大大减少了计算量，可以在低成本、低工作频率的MCU中实时运行。它确保系统以低功耗工作。为了评估IRAKF算法中一步预测卡尔曼滤波和标准扩展卡尔曼滤波的计算复杂度，计算了乘法(M)、加法和减法(A&S)、除法(D)、平方根(SR)和三角函数(T)的运算次数。附图2显示了两个卡尔曼滤波器中各操作的数量。最终在标准扩展卡尔曼滤波器中，M、A&S、D、SR和T在一秒钟内的运算次数分别为215959、203912、247、132和35。在一步预测卡尔曼滤波中，M、A&S、D、SR和T的运算次数分别为52747、39216、143、132和35。与标准的扩展卡尔曼滤波算法相比，一步预测卡尔曼滤波将M运算次数减少75.6％，A&S运算次数减少80.8％，D运算次数减少42.1％。
为了验证IRAKF算法中简化的一步预测卡尔曼滤波是否会影响组合导航系统的定位性能，在具有挑战性的城市环境中进行了实时车载实验。同时，将简化的一步预测卡尔曼滤波器替换为标准的扩展卡尔曼滤波器，并对两种算法的定位精度进行了比较。附图3显示了这两个算法在北、东和下方向的定位误差。结果显示简化的一步预测卡尔曼滤波虽然减少了算法的运算次数，但其定位精度几乎没有下降。
(2)性能验证
定位、速度、姿态精度是体现组合导航系统性能的重要指标。为了验证改进的抗差自适应卡尔曼滤波算法在复杂场景下的性能，在实时车载实验的基础上，分析了GNSS/MEMS-IMU/里程计水平定位误差、前向速度误差和航向误差。此外，也对特殊场景，即林荫道和隧道处的定位性能进行了分析和比较。
结果显示，标准卡尔曼滤波器的水平位置误差在281 989s(280000+1989)和282344s(280000+2344)之间表现出较大的值。与CRAKF、Sage滤波器、标准卡尔曼滤波器相比，本发明提出的IRAKF算法具有最高的定位精度和最佳的鲁棒性。但它仅限于大多数道路场景，主要原因是GNSS接收机无法输出观测向量协方差矩阵，人工设定的值不能准确表示当前定位误差。就水平位置误差而言，IRAKF可以提供比其他三种方法更好的性能。IRAKF的航向误差在大多数时期小于其他三种方法，误差曲线更平滑。
由CRAKF、Sage滤波、标准Kal-man滤波和IRAKF的水平位置、前进速度和航向的均方根(RMS)和最大误差的统计值可以看出，与其他三种方法相比，本发明所提出的IRAKF的均方根值和最大误差较小。具体比较分析结果如下，与CRAKF相比，IRAKF的水平位置、前进速度和航向的均方根误差分别增加了3.8％、7.9％和2.3％；与Sage滤波相比，分别提高了10.7％、10.3％和6.5％；而与标准卡尔曼滤波器相比，它分别增加了80.7％、84.1％和44.2％。就最大误差而言，与CRAKF相比，IRAKF的水平位置、前进速度和航向分别提高了1.2％、22.2％和0％；与Sage滤波相比，分别提高了2.9％、22.2％和1％；而与标准卡尔曼滤波相比，它分别增加了90.4％、95.9％和47.9％。
单频GNSS信号在林荫道上容易被遮挡，导致定位精度差。结果表示，GNSS信号在282 126秒的历元被阻断，出现异常观测。此时，GNSS定位误差达到4.12米。采用标准卡尔曼滤波算法的单频GNSS/MEMS-IMU/里程计组合导航定位误差也为3.8m。尽管CRAKF可以抑制异常的GNSS定位误差，但在减小误差发散方面仍然较差。附图4显示IARKF具有最佳精度，可防止GNSS的异常观测，并有效地减缓定位误差的发散。
据林荫道场景中，CRAKF、Sage滤波、标准卡尔曼滤波和IRAKF在GNSS异常观测时刻的水平位置误差可以得出，带有IRAKF的组合导航系统消除了GNSS粗差，提高了定位精度。与其他三种方法相比，其在GNSS粗差时的定位精度分别提高了20％、14.3％和96.8％。
隧道场景会显著影响定位精度，如果不进行处理，将出现显著误差。当车辆驶入隧道时，GNSS信号被阻断，只有里程计协助INS。然而，当车辆进入隧道时，GNSS RTK是伪固定解。如果不采取足够的措施，误差较大的GNSS伪固定解将干扰稳定的组合导航系统，导致后续定位结果出现较大误差。在出现GNSS异常值时，使用标准卡尔曼滤波进行组合导航时固定解定位误差达到2.1m，而IRAKF的定位误差仅为0.14m。该值也优于Sage滤波和CRAKF的0.165m和0.16m。此外，INS在隧道中的发散速度比其他三种方法慢。因此，IRAKF消除了RTK伪固定解，提高了系统状态的稳定性，降低了定位误差。
从隧道场景中，CRAKF、Sage滤波、标准卡尔曼滤波和IRAKF在GNSS RTK伪固定解历元的水平位置误差可得出，采用IRAKF的组合导航系统消除了GNSS异常观测，提高了定位精度。与其他三种方法相比，其在RTK伪固定解历元的定位精度分别提高了12.5％、15.2％和93.3％。
(3)与其他滤波算法测试对比
在单频GNSS/MEMS-IMU/里程计组合导航模块中验证了IRAKF算法，并与CRAKF、Sage滤波和标准卡尔曼滤波进行了比较。在整个测试中，四种算法的位置、速度和姿态的实时误差在附图5中列出。结果表示，在隧道这种复杂场景中，位置、速度和姿态误差相对较大。在这些场景中，采用标准卡尔曼滤波算法的组合导航系统难以减小由GNSS粗差引起的定位误差。采用Sage滤波和CRAKF算法的组合导航虽然可达到一定效果，但在一些恶劣环境下，其性能仍然不尽人意。然而，提出的IRAKF可以有效地消除GNSS粗差，减缓复杂环境下的系统发散误差，提高单频GNSS/MEMS-IMU/里程计组合导航的位置、速度和姿态精度。
由于标准卡尔曼滤波鲁棒性差，其位置、速度和姿态的均方根误差较大。CRAKF、Sage滤波和IRAKF能够消除粗差，它们的性能指标优于标准卡尔曼滤波。其中，测试结果表明，IRAKF具有最佳性能。就定位精度而言，与CRAKF算法相比，IRAKF在北、东和下方向分别提高了14.3％、12.5％和24.3％；与Sage滤波相比，它分别提高了10％、26.3％和17.6％；而与标准卡尔曼滤波相比，它分别提高了83.7％、78.7％和45.1％。
(4)实时功耗验证与对比
在车辆实时导航过程中，利用功率计和电源测试多传感器融合导航定位模块的功耗，并保存数据。实时功耗曲线在附图6中表示。测试结果表明，采用IRAKF算法的模块中的MCU工作频率可降低到50MHz，平均功耗为143mw。由于CRAKF、sage滤波和标准卡尔曼滤波不采用卡尔曼一步预测策略，因此MCU的工作频率只能降低到200MHz，这些方法的平均功耗相对较高，依次为296mW、300mW和304mW。结果表明，IRAKF通过降低计算复杂度可降低模块的功耗。
本文中所描述的具体实施例仅仅是对本发明精神作举例说明。本发明所属技术领域的技术人员可以对所描述的具体实施例做各种各样的修改或补充或采用类似的方式替代，但并不会偏离本发明的精神或者超越所附权利要求书所定义的范围。
Claims (10)
1.一种用于单频GNSS/MEMS-IMU/里程计低功耗实时组合导航的改进自适应抗差卡尔曼滤波算法，其特征在于：所述卡尔曼滤波预测及更新包括
卡尔曼滤波预测步骤：根据系统的模型，基于系统的上一状态而预测出现在状态，如公式(1)，再利用上一时刻的状态转移矩阵与当前时刻的状态转移矩阵相乘得到当前时刻状态转移矩阵的估计值；预测过程状态中先计算误差状态向量X，再计算状态转移矩阵Φ，公式如下：
Xk,k-1＝Φk,k-1Xk (1)
卡尔曼滤波更新步骤：根据预测步骤中的公式得到状态预测结果后，再利用量测值和预测值对当前状态进行最优估计，得到最优的状态估计值；更新过程中首先计算状态预测协方差矩阵P，然后分别计算比例因子和自适应因子，再将比例因子和自适应因子代入计算增益K的公式中，最后利用增益矩阵更新误差状态向量X和状态估计协方差矩阵P。
2.根据权利要求3所述的一种用于单频GNSS/MEMS-IMU/里程计低功耗实时组合导航的改进自适应抗差卡尔曼滤波算法，其特征在于：卡尔曼滤波更新步骤中，采用如下公式
Xk＝Xk,k-1+Kk(Zk-HkXk,k-1) (7)
Pk＝(I-KkHk)Pk,k-1 (8)
其中，X是误差状态向量；Φ表示状态转移矩阵；P是状态预测协方差矩阵；Q是系统噪声协方差矩阵；K是卡尔曼滤波增益矩阵，它决定观测信息的权重；H表示设计矩阵；R代表观测噪声协方差矩阵；Z代表观测向量；α是比例因子；β是自适应因子；trace()表示对矩阵求迹；
表示卡方分布；/>
是预测残差向量；以上的下角标代表状态的变化，例如，Xk,k-1表示从k-1历元到k历元的误差状态向量。
3.根据权利要求2所述的一种用于单频GNSS/MEMS-IMU/里程计低功耗实时组合导航的改进自适应抗差卡尔曼滤波算法，其特征在于：当GNSS RTK为固定解时，计算比例因子根据以下公式
其中，
是固定解的观测向量协方差矩阵。
4.根据权利要求3所述的一种用于单频GNSS/MEMS-IMU/里程计低功耗实时组合导航的改进自适应抗差卡尔曼滤波算法，其特征在于：当GNSS RTK为浮点解时，计算比例因子根据以下公式
其中，
是浮点解的观测向量协方差矩阵。
5.根据权利要求3所述的一种低成本嵌入式GNSS/INS组合导航时间同步算法，其特征在于：当GNSS为伪距差分解时，计算比例因子根据以下公式
其中，
是浮点解的观测向量协方差矩阵。
6.根据权利要求3所述的一种用于单频GNSS/MEMS-IMU/里程计低功耗实时组合导航的改进自适应抗差卡尔曼滤波算法，其特征在于：计算自适应因子根据以下公式
βk(i)的初始值βk(0)＝1。
7.一种用于单频GNSS/MEMS-IMU/里程计低功耗实时组合导航的改进自适应抗差卡尔曼滤波系统，其特征在于：包括
卡尔曼滤波预测模块：根据系统的模型，基于系统的上一状态而预测出现在状态，如公式(1)，再利用上一时刻的状态转移矩阵与当前时刻的状态转移矩阵相乘得到当前时刻状态转移矩阵的估计值；预测过程状态中先计算误差状态向量X，再计算状态转移矩阵Φ，公式如下：
Xk,k-1＝Φk,k-1Xk (1)
卡尔曼滤波更新模块：根据预测步骤中的公式得到状态预测结果后，再利用量测值和预测值对当前状态进行最优估计，得到最优的状态估计值；更新过程中首先计算状态预测协方差矩阵P，然后分别计算比例因子和自适应因子，再将比例因子和自适应因子代入计算增益K的公式中，最后利用增益矩阵更新误差状态向量X和状态估计协方差矩阵P。
8.根据权利要求3所述的一种用于单频GNSS/MEMS-IMU/里程计低功耗实时组合导航的改进自适应抗差卡尔曼滤波算法，其特征在于：卡尔曼滤波更新模块中，采用如下公式
Xk＝Xk,k-1+Kk(Zk-HkXk,k-1) (7)
Pk＝(I-KkHk)Pk,k-1 (8)
其中，X是误差状态向量；Φ表示状态转移矩阵；P是状态预测协方差矩阵；Q是系统噪声协方差矩阵；K是卡尔曼滤波增益矩阵，它决定观测信息的权重；H表示设计矩阵；R代表观测噪声协方差矩阵；Z代表观测向量；α是比例因子；β是自适应因子；trace()表示对矩阵求迹；
表示卡方分布；/>
是预测残差向量；以上的下角标代表状态的变化，例如，Xk,k-1表示从k-1历元到k历元的误差状态向量。
9.根据权利要求2所述的一种用于单频GNSS/MEMS-IMU/里程计低功耗实时组合导航的改进自适应抗差卡尔曼滤波算法，其特征在于：当GNSS RTK为固定解时，计算比例因子根据以下公式
其中，
是固定解的观测向量协方差矩阵；
当GNSS RTK为浮点解时，计算比例因子根据以下公式
其中，
是浮点解的观测向量协方差矩阵；
当GNSS为伪距差分解时，计算比例因子根据以下公式
其中，
是浮点解的观测向量协方差矩阵。
10.根据权利要求3所述的一种用于单频GNSS/MEMS-IMU/里程计低功耗实时组合导航的改进自适应抗差卡尔曼滤波算法，其特征在于：计算自适应因子根据以下公式
βk(i)的初始值βk(0)＝1。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202310340961.6A CN116150565A (zh) | 2023-03-31 | 2023-03-31 | 一种用于单频gnss/mems-imu/里程计低功耗实时组合导航的改进自适应抗差卡尔曼滤波算法及系统 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202310340961.6A CN116150565A (zh) | 2023-03-31 | 2023-03-31 | 一种用于单频gnss/mems-imu/里程计低功耗实时组合导航的改进自适应抗差卡尔曼滤波算法及系统 |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN116150565A true CN116150565A (zh) | 2023-05-23 |
Family
ID=86350826
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202310340961.6A Pending CN116150565A (zh) | 2023-03-31 | 2023-03-31 | 一种用于单频gnss/mems-imu/里程计低功耗实时组合导航的改进自适应抗差卡尔曼滤波算法及系统 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN116150565A (zh) |
Cited By (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN116358566A (zh) * | 2023-06-01 | 2023-06-30 | 山东科技大学 | 一种基于抗差自适应因子的粗差探测组合导航方法 |
-
2023
- 2023-03-31 CN CN202310340961.6A patent/CN116150565A/zh active Pending
Cited By (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN116358566A (zh) * | 2023-06-01 | 2023-06-30 | 山东科技大学 | 一种基于抗差自适应因子的粗差探测组合导航方法 |
| CN116358566B (zh) * | 2023-06-01 | 2023-08-11 | 山东科技大学 | 一种基于抗差自适应因子的粗差探测组合导航方法 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN110823217B (zh) | 一种基于自适应联邦强跟踪滤波的组合导航容错方法 | |
| Zhao et al. | High-precision vehicle navigation in urban environments using an MEM's IMU and single-frequency GPS receiver | |
| US8773303B2 (en) | Position tracking device and method | |
| US10317421B2 (en) | Positioning apparatus comprising an inertial sensor and inertial sensor temperature compensation method | |
| US8560234B2 (en) | System and method of navigation based on state estimation using a stepped filter | |
| US6167347A (en) | Vehicle positioning method and system thereof | |
| US7193559B2 (en) | Inertial GPS navigation system with modified kalman filter | |
| CN108415042B (zh) | 改善gnss接收机载波相位连续性的相位预测方法及系统 | |
| US11300689B2 (en) | System and method for reconverging GNSS position estimates | |
| Georgy et al. | Vehicle navigator using a mixture particle filter for inertial sensors/odometer/map data/GPS integration | |
| CA3003298A1 (en) | Gnss and inertial navigation system utilizing relative yaw as an observable for an ins filter | |
| CN102252677A (zh) | 一种基于时间序列分析的变比例自适应联邦滤波方法 | |
| Giremus et al. | A Rao-Blackwellized particle filter for INS/GPS integration | |
| EP3312634B1 (en) | Positioning apparatus | |
| Jwo et al. | Adaptive Kalman filter for navigation sensor fusion | |
| CN114562992B (zh) | 一种基于因子图及场景约束的多径环境组合导航方法 | |
| CN102819029A (zh) | 一种超紧组合卫星导航接收机 | |
| Hide et al. | GPS and low cost INS integration for positioning in the urban environment | |
| US20230341563A1 (en) | System and method for computing positioning protection levels | |
| CN116150565A (zh) | 一种用于单频gnss/mems-imu/里程计低功耗实时组合导航的改进自适应抗差卡尔曼滤波算法及系统 | |
| CN113252048A (zh) | 一种导航定位方法、导航定位系统及计算机可读存储介质 | |
| CN116399351A (zh) | 一种车辆位置估计方法 | |
| CN115902963A (zh) | 一种单点定位数据处理方法、装置、电子设备及存储介质 | |
| CN116224407B (zh) | 一种gnss和ins组合导航定位方法及系统 | |
| CN103727947A (zh) | 基于ukf滤波的bds与gis深耦合定位方法和系统 |
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 |