CN114858166B - 基于最大相关熵卡尔曼滤波器的imu姿态解算方法 - Google Patents

基于最大相关熵卡尔曼滤波器的imu姿态解算方法 Download PDF

Info

Publication number
CN114858166B
CN114858166B CN202210807273.1A CN202210807273A CN114858166B CN 114858166 B CN114858166 B CN 114858166B CN 202210807273 A CN202210807273 A CN 202210807273A CN 114858166 B CN114858166 B CN 114858166B
Authority
CN
China
Prior art keywords
entropy
state
imu attitude
error
imu
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
Application number
CN202210807273.1A
Other languages
English (en)
Other versions
CN114858166A (zh
Inventor
付博
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Beijing Shendao Technology Co ltd
Original Assignee
Beijing Shendao Technology Co ltd
Priority date (The priority date 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 date listed.)
Filing date
Publication date
Application filed by Beijing Shendao Technology Co ltd filed Critical Beijing Shendao Technology Co ltd
Priority to CN202210807273.1A priority Critical patent/CN114858166B/zh
Publication of CN114858166A publication Critical patent/CN114858166A/zh
Application granted granted Critical
Publication of CN114858166B publication Critical patent/CN114858166B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

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
    • 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
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/20—Analysis of motion
    • G06T7/277—Analysis of motion involving stochastic approaches, e.g. using Kalman filters

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Multimedia (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • Theoretical Computer Science (AREA)
  • Navigation (AREA)

Abstract

本发明涉及一种基于最大相关熵卡尔曼滤波器的IMU姿态解算方法，属于惯性导航的姿态解算技术领域，解决解算精度问题，包括，建立两个随机变量的多核相关熵；所述多核相关熵为基于N次采样的包括p个高斯核的相关熵；建立IMU姿态解算的线性系统模型；对于所述线性系统模型，建立在多核相关熵下的最大相关熵卡尔曼滤波器；通过卡尔曼滤波进行IMU进行姿态解算。本发明提升了姿态解算的精度。

Description

基于最大相关熵卡尔曼滤波器的IMU姿态解算方法
技术领域
本发明属于惯性导航的姿态解算技术领域，具体涉及一种基于最大相关熵卡尔曼滤波器的IMU姿态解算方法。
背景技术
IMU（Inertial Measurement Unit，惯性测量单元）被广泛用于载体的姿态测量，一般由三轴的陀螺和三轴加速度计组成，特别是在智能机器领域，载体的运动跟踪和定位主要依靠IMU来确定。但是由于低成本的惯性器件会产生漂移、抗干扰的能力较差，导致精度很低。通常的做法是设计多传感器的融合算法，即引入磁力计，利用加速度计和磁力计的信息矫正陀螺仪的漂移，同时也利用动态下的陀螺输出修正加速度计结合磁力计的测量，消除磁场干扰等因素的影响。但是对于消除姿态解算过程中的非高斯噪声和未知干扰等方面的影响就非常有限。
MCC（Maximum Correntropy Criterion，最大相关熵准则）已经开始应用于卡尔曼滤波器中，对消除非高斯噪声和干扰有明显的优势。
利用MCKF（maximum correntropy Kalman filter，最大相关熵卡尔曼滤波器）进行IMU进行姿态解算，能够提升系统对于异常值和非高斯噪声的鲁棒性，从而进一步提升姿态解算的精度。
发明内容
鉴于上述的分析，本发明旨在公开了一种基于最大相关熵卡尔曼滤波器的IMU姿态解算方法，用于提升姿态解算的精度。
本发明公开了一种基于最大相关熵卡尔曼滤波器的IMU姿态解算方法，包括，
建立两个随机变量的多核相关熵；所述多核相关熵为基于N次采样的包括p个高斯核的相关熵；
建立IMU姿态解算的线性系统模型；
对于所述线性系统模型，建立在多核相关熵下的最大相关熵卡尔曼滤波器；
通过卡尔曼滤波进行IMU进行姿态解算。
进一步地，所述多核相关熵的表达式为：
Figure 997483DEST_PATH_IMAGE001
；
其中，X,Y∈R p ，为两个一维随机变量；N为采样次数；k=1,…N；
Figure 889216DEST_PATH_IMAGE002
为高斯函数，σ i 为第i个元素的高斯核带宽，i=1,…p，p为高斯核的个数。
进一步地，建立IMU姿态解算的线性系统模型如下：
Figure 807493DEST_PATH_IMAGE003
；
其中
Figure 204058DEST_PATH_IMAGE004
∈Rn是IMU姿态解算的状态向量，
Figure 625812DEST_PATH_IMAGE005
∈Rm是IMU姿态解算的观测向量，
Figure 954026DEST_PATH_IMAGE006
是状态转移矩阵，H是观测矩阵，q k 和r k 为假设彼此独立的白噪声；q k 的协方差矩阵为
Figure 94020DEST_PATH_IMAGE007
；r k 的协方差矩阵为
Figure 798671DEST_PATH_IMAGE008
。
进一步地，将建立IMU姿态解算的线性系统模型描述为：
Figure 340511DEST_PATH_IMAGE009
其中I∈Rn×n是单位矩阵，x k —是x k 的先验估计；噪声项ν k 是：
Figure 839625DEST_PATH_IMAGE010
；
噪声项ν k 的协方差矩阵为
Figure 466915DEST_PATH_IMAGE011
；
其中，
Figure 211143DEST_PATH_IMAGE012
是先验误差协方差，B p 和B r 为对
Figure 607489DEST_PATH_IMAGE012
和
Figure 543084DEST_PATH_IMAGE013
进行Cholesky分解获得；
在
Figure 657671DEST_PATH_IMAGE014
两边左乘Bk −1得到
Figure 438545DEST_PATH_IMAGE015
；
其中，
Figure 954977DEST_PATH_IMAGE016
；ξ k 为白噪声，E(ξ k ξ k ’)=I。
进一步地，所述多核相关熵下的最大相关熵卡尔曼滤波器的MCC目标函数：
Figure 795894DEST_PATH_IMAGE017
G σi (e i,k )的高斯函数，σ i 为内核带宽，e i,k =τ i,k −w i,k x k 是预测值和真实值之间的误差，步长k，τ i,k 为T k 的第i个元素，w i,k 是W k 的第i行，n为状态向量的维度，m为观测向量的维度。
进一步地，在MCC下，最优估计量是：
Figure 132197DEST_PATH_IMAGE018
。
进一步地，满足最优解
Figure 483806DEST_PATH_IMAGE019
的状态向量
Figure 854745DEST_PATH_IMAGE020
；
其中，
Figure 600984DEST_PATH_IMAGE021
；
Figure 424583DEST_PATH_IMAGE022
；
Figure 812840DEST_PATH_IMAGE023
；
Figure 303864DEST_PATH_IMAGE024
；
σ p =[σ1,σ2,...,σn]’是过程带宽向量，σr=[σn+1,σn+2,...,σn+m]’是测量带宽向量，e p 是过程误差，e r 是测量误差。
进一步地，通过卡尔曼滤波进行IMU进行姿态解算的过程包括：
进行姿态解算的初始化；
利用建立的IMU姿态解算的线性系统模型进行状态预测；
利用预测的状态进行初始时刻的状态更新；
进入卡尔曼滤波；根据更新的状态，判断是否满足滤波的约束条件；如果不满足，则执行卡尔曼滤波过程，对于滤波后的状态更新，继续判断是否满足滤波的约束条件；如果不满足继续执行卡尔曼滤波过程；如果满足，则进入到下一步；
更新后验误差，输出解算结果。
进一步地，所述多核相关熵下的最大相关熵卡尔曼滤波器的滤波过程包括：
Figure 486583DEST_PATH_IMAGE025
；
其中，
Figure 564523DEST_PATH_IMAGE026
为t时刻的状态更新值，
Figure 490891DEST_PATH_IMAGE027
为t-1时刻的状态更新值，
Figure 305263DEST_PATH_IMAGE028
为状态的预测值，
Figure 190042DEST_PATH_IMAGE029
为t时刻的增益估计值，
Figure 722655DEST_PATH_IMAGE030
为先验误差协方差
Figure 452713DEST_PATH_IMAGE031
的估计值，
Figure 387171DEST_PATH_IMAGE032
为协方差矩阵
Figure 177273DEST_PATH_IMAGE033
的估计值，
Figure 462761DEST_PATH_IMAGE034
为M p 的估计值，
Figure 226537DEST_PATH_IMAGE035
为M r 的估计值，e p 为过程误差，e r 为测量误差。
进一步地，所述多核相关熵下的最大相关熵卡尔曼滤波器的约束条件为：
Figure 281080DEST_PATH_IMAGE036
；
其中，
Figure 976504DEST_PATH_IMAGE037
为t时刻的状态更新值，
Figure 483708DEST_PATH_IMAGE038
为t-1时刻的状态更新值，
Figure 555570DEST_PATH_IMAGE039
为约束阈值，α和β为两个小的正数，在初始化时进行设置。
本发明至少可实现以下有益效果之一：
本发明的基于最大相关熵卡尔曼滤波器的IMU姿态解算方法，增强惯性系统的鲁棒性，能够抑制系统姿态解算过程中的异常值、非高斯噪声和未知干扰；提升了系统的精度，能够可靠的估计陀螺的偏值，同时过滤来自加速度计和磁力计的输出异常值。
附图说明
附图仅用于示出具体实施例的目的，而并不认为是对本发明的限制，在整个附图中，相同的参考符号表示相同的部件：
图1为本发明实施例中的基于最大相关熵卡尔曼滤波器的IMU姿态解算方法流程图；
图2为本发明实施例中的MCKF算法流程图；
图3为本发明实施例中的加入非高斯噪声后比较GD、标准姿态解算和MCKF三种算法解算的方位角对比图；
图4为本发明实施例中的加入非高斯噪声后比较GD、标准姿态解算和MCKF三种算法解算的俯仰角对比图；
图5为本发明实施例中的加入非高斯噪声后比较GD、标准姿态解算和MCKF三种算法解算的横滚角对比图；
图6为本发明实施例中的加入非高斯噪声后比较GD、标准姿态解算和MCKF三种算法解算的三轴加速度计的输出对比图；
图7为本发明实施例中的加入非高斯噪声后比较GD、标准姿态解算和MCKF三种算法解算的三轴陀螺仪的输出对比图；
图8为本发明实施例中的加入非高斯噪声后比较GD、标准姿态解算和MCKF三种算法解算的三轴磁力计对比图。
具体实施方式
下面结合附图来具体描述本发明的优选实施例，其中，附图构成本申请一部分，并与本发明的实施例一起用于阐释本发明的原理。
本发明的一个实施例公开了一种基于最大相关熵卡尔曼滤波器的IMU姿态解算方法如图1所示，包括：
步骤S1、建立两个随机变量的多核相关熵；所述多核相关熵为基于N次采样的包括p个高斯核的相关熵；
步骤S2、建立IMU姿态解算的线性系统模型；
步骤S3、对于所述线性系统模型，建立在多核相关熵下的最大相关熵卡尔曼滤波器；
步骤S4、通过卡尔曼滤波进行IMU进行姿态解算。
具体的，在步骤S1中，定义衡量两个随机变量的局部相似性为熵。给定两个随机变量X,Y∈R，联合分布函数F XY (x,y),相关熵为：
Figure 464620DEST_PATH_IMAGE040
（式1）
其中，
Figure 330945DEST_PATH_IMAGE041
是满足Mercer定理的核函数，通常首选为高斯核函数，因此定义为：
Figure 591025DEST_PATH_IMAGE042
（式2）
在本实施例中，将相关熵的定义从随机变量扩展到随机向量。给定两个随机向量X,Y∈Rp，相关熵定义为每个元素的加权和相关熵，如下式：
Figure 935418DEST_PATH_IMAGE043
（式3）
其中，
Figure 466019DEST_PATH_IMAGE044
（式4）
Figure 237666DEST_PATH_IMAGE045
是高斯函数，σi是第i个元素的核带宽，并且e i =x i -y i 是第i个误差。
可以看出，式3为两个随机向量定义了一个新的局部相似性度量，其中，核向量σ=[σ1,σ2,…,σp]’控制“观察窗口”和加权系数，’表示向量的转置。
在许多应用中，样本是有限的，联合分布是未知的。基于此，将式3简化后的所述多核相关熵的表达式为：
Figure 516201DEST_PATH_IMAGE046
（式5）
其中，X,Y∈R p ，为两个一维随机变量；N为采样次数；k=1,…N；
Figure 929864DEST_PATH_IMAGE002
为高斯函数，σ i 为第i个元素的高斯核带宽，i=1,…p，p为高斯核的个数。其中e i (k) =x i (k)-y i (k)是元素i在第k次采样时的误差；x i (k)、y i (k)是一维随机变量X、Y的第i个元素在第k次采样时的采样值。
首先对IMU建模，将陀螺和加速度计输出分解为真值、零偏和高斯噪声三部分。如式6所示：
Figure 813507DEST_PATH_IMAGE047
Figure 21634DEST_PATH_IMAGE048
(式6)
其中，
Figure 990727DEST_PATH_IMAGE049
和
Figure 942503DEST_PATH_IMAGE050
是陀螺和加速度计的实际输出，
Figure 680652DEST_PATH_IMAGE051
和
Figure 561145DEST_PATH_IMAGE052
为角速度和比力真值，
Figure 17534DEST_PATH_IMAGE053
和
Figure 507421DEST_PATH_IMAGE054
是陀螺和加速度计的白噪声，
Figure 365656DEST_PATH_IMAGE055
和
Figure 181165DEST_PATH_IMAGE056
是零偏一般与温度有关，因此可以对零偏的变化建模如下：
Figure 859271DEST_PATH_IMAGE057
(式7)
其中，
Figure 152849DEST_PATH_IMAGE058
和
Figure 865591DEST_PATH_IMAGE059
是高斯白噪声。
随后，对惯性运动建模，分为姿态解算（方位、俯仰和横滚）、速度解算和位置解算，具体如下：
Figure 586422DEST_PATH_IMAGE060
Figure 518868DEST_PATH_IMAGE061
(式8)
其中，
Figure 350558DEST_PATH_IMAGE062
为姿态角由陀螺输出角增量通过等效旋转式量法获得更新的姿态误差，exp（×）是反对称矩阵的指数函数。
Figure 917805DEST_PATH_IMAGE063
为导航坐标系下的东北天速度由减去重力加速度g后的加速度积分获得更新的速度误差，
Figure 75117DEST_PATH_IMAGE064
是经纬高位置由东北天速度积分获得的位置误差。
然后，对卡尔曼滤波进行建模，状态变量选择为：
Figure 993395DEST_PATH_IMAGE065
(公式9)
其中，
Figure 363196DEST_PATH_IMAGE062
姿态误差，
Figure 50529DEST_PATH_IMAGE063
为速度误差，
Figure 378743DEST_PATH_IMAGE064
为位置误差，
Figure 253158DEST_PATH_IMAGE066
为陀螺零漂、
Figure 957808DEST_PATH_IMAGE067
为加速度计零位,共15维。
具体的，步骤S2中建立的建立IMU姿态解算的线性系统模型如下：
Figure 18691DEST_PATH_IMAGE068
（式10）
其中
Figure 517806DEST_PATH_IMAGE069
∈Rn是IMU姿态解算的状态向量，
Figure 879517DEST_PATH_IMAGE070
∈Rm是IMU姿态解算的观测向量，
Figure 856700DEST_PATH_IMAGE071
是状态转移矩阵，H是观测矩阵，q k 和r k 为假设彼此独立的白噪声；q k 的协方差矩阵为
Figure 518626DEST_PATH_IMAGE072
；r k 的协方差矩阵为
Figure 923062DEST_PATH_IMAGE073
。
更具体的，所述IMU姿态解算的状态向量可包括由IMU三轴姿态、速度、位置、三轴陀螺的零偏误差、三轴加速度的零偏误差等状态量组成的状态向量；
所述IMU姿态解算的观测向量可包括例如磁力计、里程计、卫星接收机在内的其他传感器所观测的三轴姿态，速度、位置与IMU解算的三轴速度、位置结果的观测误差。
在本实施例中，通过多核相关熵推导出了一个新的滤波器，多核相关熵下的最大相关熵卡尔曼滤波器（MCKF，maximum correntropy Kalman filter）；
将建立IMU姿态解算的线性系统模型描述为：
Figure 37649DEST_PATH_IMAGE074
（式11）
其中I∈Rn×n是单位矩阵，x k 是状态，x k —是状态的先验估计；噪声项ν k 是：
Figure 818523DEST_PATH_IMAGE075
（式12）
噪声项ν k 的协方差矩阵为
Figure 69376DEST_PATH_IMAGE076
（式13）
其中，
Figure 411758DEST_PATH_IMAGE077
是先验误差协方差，B p 和B r 为对
Figure 13640DEST_PATH_IMAGE077
和
Figure 332626DEST_PATH_IMAGE078
进行Cholesky分解获得；
在
Figure 703565DEST_PATH_IMAGE079
两边左乘Bk −1得到
Figure 715383DEST_PATH_IMAGE080
；
其中，
Figure 273403DEST_PATH_IMAGE081
（式14）
ξ k 为白噪声，E(ξ k ξ k ’)=I。
所述多核相关熵下的最大相关熵卡尔曼滤波器的MCC的目标函数：
Figure 661659DEST_PATH_IMAGE082
（式15）
G σi (e i,k )的高斯函数，σ i 为内核带宽，e i,k =τ i,k −w i,k x k 是预测值和真实值之间的误差，步长k，τ i,k 为T k 的第i个元素，w i,k 是W k 的第i行，n为状态向量的维度，m为观测向量的维度。
在MCC下，最优估计量是
Figure 887104DEST_PATH_IMAGE083
（式16）
为了获得最优解需要满足：
Figure 69824DEST_PATH_IMAGE084
Figure 380719DEST_PATH_IMAGE085
（式17）
所以：
Figure 808552DEST_PATH_IMAGE086
（式18）
把，e i,k =t i,k −w i,k x k 代入上式，得到：
Figure 888503DEST_PATH_IMAGE087
（式19）
Figure 242124DEST_PATH_IMAGE088
（式20）
σp=[σ1,σ2,...,σn]’是过程带宽向量，σr=[σn+1,σn+2,...,σn+m]’是测量带宽向量，e p 是过程误差，e r 是测量误差。带宽向量中的每个带宽的选择决定了对应状态的核密度估计的平滑程度，在对高斯核函数进行密度估计时，带宽𝜎i的最优选择满足使平均积分平方误差最小化。
将（式13）和（式14）代入（式19），我们得到：
Figure 40316DEST_PATH_IMAGE089
（式21）
然后，对矩阵求逆并简化，我们得到：
Figure 239216DEST_PATH_IMAGE090
Figure 439254DEST_PATH_IMAGE091
（式22）
代入（式19）得到：
Figure 229355DEST_PATH_IMAGE092
（式23）
其中，
Figure 983684DEST_PATH_IMAGE093
（式24）
最后，后验误差协方差可以由下式进行更新：
Figure 517434DEST_PATH_IMAGE094
（式25）
因为
Figure 306398DEST_PATH_IMAGE095
和
Figure 503287DEST_PATH_IMAGE096
与xk相关。因此，可以使用定点算法来解算。
为了避免M p 和M r 的奇异性，我们引入以下约束：
Figure 276071DEST_PATH_IMAGE097
（式26）
其中α和β是两个小的正数。在实际应用中，如果对角矩阵Mp和Mr的元素小于α或β，则相应地用标量α或β代替。然后（式19）将停止迭代。
具体的，通过卡尔曼滤波进行IMU进行姿态解算的过程如图2所示，包括：
1）初始化步骤；进行姿态解算的初始化；
对包括过程带宽向量σ p 、测量带宽向量σ r 、α和β在内的量进行初始化；
2）状态初始预测步骤；利用建立的IMU姿态解算的线性系统模型进行状态预测；
进行初始状态预测：
Figure 82353DEST_PATH_IMAGE098
；
并对先验误差协方差
Figure 991403DEST_PATH_IMAGE077
和观测噪声的协方差矩阵
Figure 123307DEST_PATH_IMAGE099
进行Cholesky分解获得B p 和B r ；
3）状态初始更新步骤；利用预测的状态进行初始时刻的状态更新；
Figure 852228DEST_PATH_IMAGE100
；
4）卡尔曼滤波步骤；进入卡尔曼滤波；根据更新的状态，判断是否满足滤波的约束条件；如果不满足，则执行卡尔曼滤波过程，对于滤波后的状态更新，继续判断是否满足滤波的约束条件；如果不满足继续执行卡尔曼滤波过程；如果满足，则进入到下一步；
具体的，卡尔曼滤波过程包括：
Figure 727781DEST_PATH_IMAGE101
；
其中，
Figure 491337DEST_PATH_IMAGE102
为t时刻的状态更新值，
Figure 528563DEST_PATH_IMAGE103
为t-1时刻的状态更新值，
Figure 10360DEST_PATH_IMAGE104
为状态的预测值，
Figure 919630DEST_PATH_IMAGE105
为t时刻的增益估计值，
Figure 803272DEST_PATH_IMAGE106
为先验误差协方差
Figure 745820DEST_PATH_IMAGE077
的估计值，
Figure 714913DEST_PATH_IMAGE107
为协方差矩阵
Figure 666689DEST_PATH_IMAGE108
的估计值，
Figure 404838DEST_PATH_IMAGE109
为M p 的估计值，
Figure 49446DEST_PATH_IMAGE110
为M r 的估计值，e p 为过程误差，e r 为测量误差。
具体的，约束条件为：
Figure 240255DEST_PATH_IMAGE111
；
其中，
Figure 995722DEST_PATH_IMAGE102
为t时刻的状态更新值，
Figure 355421DEST_PATH_IMAGE103
为t-1时刻的状态更新值，
Figure 905351DEST_PATH_IMAGE112
为约束阈值，α和β为两个小的正数，在初始化时进行设置；
5）更新后验误差，输出解算结果。
具体的，按照式25进行后验误差的更新。
在本实施例中，为了验证梯度下降算法（GD）、标准姿态解算算法（AHRS）和本实施例的最大相关熵卡尔曼滤波算法（MCKF）的性能，我们设计了一组仿真试验，人为的加入非高斯噪声：
Figure 583457DEST_PATH_IMAGE113
（式27）
其中，
Figure 877035DEST_PATH_IMAGE114
（式28）
通过仿真对比，发现本实施例的MCKF算法的鲁棒性明显优于前二者，特别是在方位角和俯仰角的解算能力上优势明显，如图3~8所示的加入非高斯噪声后比较GD、标准姿态解算和MCKF三种算法解算的方位角、俯仰角、横滚角、三轴加速度计输出、三轴陀螺输出和三轴磁力计输出的对比图。
综上所述，本实施例的基于最大相关熵卡尔曼滤波器的IMU姿态解算方法，增强惯性系统的鲁棒性，能够抑制系统姿态解算过程中的异常值、非高斯噪声和未知干扰；提升了系统的精度，能够可靠的估计陀螺的偏值，同时过滤来自加速度计和磁力计的输出异常值。
以上所述，仅为本发明较佳的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，可轻易想到的变化或替换，都应涵盖在本发明的保护范围之内。

Claims (6)

1.一种基于最大相关熵卡尔曼滤波器的IMU姿态解算方法，其特征在于，包括，
建立两个随机变量的多核相关熵；所述多核相关熵为基于N次采样的包括p个高斯核的相关熵；
所述多核相关熵的表达式为：
Figure 738354DEST_PATH_IMAGE002
；
其中，X,Y∈R p ，为两个一维随机变量；N为采样次数；k=1,…N；
Figure 858757DEST_PATH_IMAGE003
为高斯函数，σ i 为第i个元素的高斯核带宽，i=1,…p，p为高斯核的个数，e i (k) =x i (k)-y i (k)是第i个元素在第k次采样时的误差，x i (k)、y i (k)是一维随机变量X、Y的第i个元素在第k次采样时的采样值；
建立IMU姿态解算的线性系统模型；建立IMU姿态解算的线性系统模型如下：
Figure 372915DEST_PATH_IMAGE005
；
其中
Figure 925381DEST_PATH_IMAGE007
∈Rn是IMU姿态解算的状态向量，
Figure 536491DEST_PATH_IMAGE008
∈Rm是IMU姿态解算的观测向量，
Figure 765478DEST_PATH_IMAGE009
是状态转移矩阵，H是观测矩阵，q k 和r k 为假设彼此独立的白噪声；q k 的协方差矩阵为
Figure 766932DEST_PATH_IMAGE011
；r k 的协方差矩阵为
Figure 496991DEST_PATH_IMAGE012
；
将建立IMU姿态解算的线性系统模型描述为：
Figure 821662DEST_PATH_IMAGE013
其中I∈Rn×n是单位矩阵，x k —是x k 的先验估计；噪声项ν k 是：
Figure 346184DEST_PATH_IMAGE015
；
噪声项ν k 的协方差矩阵为：
Figure 772617DEST_PATH_IMAGE017
；
其中，
Figure 40787DEST_PATH_IMAGE018
是先验误差协方差，B p 和B r 为对
Figure 298593DEST_PATH_IMAGE018
和
Figure 882765DEST_PATH_IMAGE019
进行Cholesky分解获得；
在
Figure 921128DEST_PATH_IMAGE013
两边左乘Bk −1得到
Figure 602777DEST_PATH_IMAGE021
；
其中，
Figure 980668DEST_PATH_IMAGE022
；ξ k 为白噪声，E(ξ k ξ k ’)=I；
对于所述线性系统模型，建立在多核相关熵下的最大相关熵卡尔曼滤波器；
所述多核相关熵下的最大相关熵卡尔曼滤波器的MCC目标函数：
Figure 846993DEST_PATH_IMAGE024
Figure 497286DEST_PATH_IMAGE025
为高斯函数，e i,k =τ i,k −w i,k x k 是预测值和真实值之间的误差，τ i,k 为T k 的第i个元素，w i,k 是W k 的第i行，n为状态向量的维度，m为观测向量的维度；
通过卡尔曼滤波进行IMU姿态解算。
2.根据权利要求1所述的IMU姿态解算方法，其特征在于，
在MCC下，最优估计量是：
Figure 107259DEST_PATH_IMAGE027
。
3.根据权利要求2所述的IMU姿态解算方法，其特征在于，满足最优解
Figure 11761DEST_PATH_IMAGE029
的状态向量
Figure 517829DEST_PATH_IMAGE031
；
其中，
Figure 265205DEST_PATH_IMAGE033
；
Figure 304967DEST_PATH_IMAGE034
；
Figure 391872DEST_PATH_IMAGE035
；
Figure 865579DEST_PATH_IMAGE036
；
σ p =[σ1,σ2,...,σn]’是过程带宽向量，σr=[σn+1,σn+2,...,σn+m]’是测量带宽向量，e p 是过程误差，e r 是测量误差。
4.根据权利要求3所述的IMU姿态解算方法，其特征在于，通过卡尔曼滤波进行IMU进行姿态解算的过程包括：
进行姿态解算的初始化；
利用建立的IMU姿态解算的线性系统模型进行状态预测；
利用预测的状态进行初始时刻的状态更新；
进入卡尔曼滤波；根据更新的状态，判断是否满足滤波的约束条件；如果不满足，则执行多核相关熵下的最大相关熵卡尔曼滤波器的滤波过程，对于滤波后的状态更新，继续判断是否满足滤波的约束条件；如果不满足继续执行卡尔曼滤波过程；如果满足，则进入到下一步；
更新后验误差，输出解算结果。
5.根据权利要求4所述的IMU姿态解算方法，其特征在于，所述多核相关熵下的最大相关熵卡尔曼滤波器的滤波过程包括：
Figure 975617DEST_PATH_IMAGE038
；
其中，
Figure 927393DEST_PATH_IMAGE039
为t时刻的状态更新值，
Figure 134383DEST_PATH_IMAGE040
为t-1时刻的状态更新值，
Figure 903625DEST_PATH_IMAGE041
为状态的预测值，
Figure 625593DEST_PATH_IMAGE042
为t时刻的增益估计值，
Figure 256426DEST_PATH_IMAGE043
为先验误差协方差
Figure 849081DEST_PATH_IMAGE044
的估计值，
Figure 842355DEST_PATH_IMAGE045
为协方差矩阵
Figure 661407DEST_PATH_IMAGE046
的估计值，
Figure 423826DEST_PATH_IMAGE047
为M p 的估计值，
Figure 136568DEST_PATH_IMAGE048
为M r 的估计值，e p 为过程误差，e r 为测量误差。
6.根据权利要求5所述的IMU姿态解算方法，其特征在于，所述多核相关熵下的最大相关熵卡尔曼滤波器的约束条件为：
Figure 247612DEST_PATH_IMAGE049
；
其中，
Figure 678593DEST_PATH_IMAGE050
为t时刻的状态更新值，
Figure 651228DEST_PATH_IMAGE051
为t-1时刻的状态更新值，
Figure 687318DEST_PATH_IMAGE052
为约束阈值，α和β为两个小的正数，在初始化时进行设置。
CN202210807273.1A 2022-07-11 2022-07-11 基于最大相关熵卡尔曼滤波器的imu姿态解算方法 Active CN114858166B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202210807273.1A CN114858166B (zh) 2022-07-11 2022-07-11 基于最大相关熵卡尔曼滤波器的imu姿态解算方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202210807273.1A CN114858166B (zh) 2022-07-11 2022-07-11 基于最大相关熵卡尔曼滤波器的imu姿态解算方法

Publications (2)

Publication Number Publication Date
CN114858166A CN114858166A (zh) 2022-08-05
CN114858166B true CN114858166B (zh) 2022-10-11

Family

ID=82626506

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202210807273.1A Active CN114858166B (zh) 2022-07-11 2022-07-11 基于最大相关熵卡尔曼滤波器的imu姿态解算方法

Country Status (1)

Country Link
CN (1) CN114858166B (zh)

Families Citing this family (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN117705108A (zh) * 2023-12-12 2024-03-15 中铁第四勘察设计院集团有限公司 一种基于最大相关熵的滤波定位方法、系统及滤波器

Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN105737828A (zh) * 2016-05-09 2016-07-06 郑州航空工业管理学院 一种基于强跟踪的相关熵扩展卡尔曼滤波的组合导航方法
CN113792412A (zh) * 2021-08-13 2021-12-14 中国人民解放军军事科学院国防科技创新研究院 一种基于中心误差熵准则容积卡尔曼滤波的航天器姿态确定方法
CN113792411A (zh) * 2021-08-13 2021-12-14 中国人民解放军军事科学院国防科技创新研究院 一种基于中心误差熵准则无迹卡尔曼滤波的航天器姿态确定方法

Family Cites Families (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN106291645B (zh) * 2016-07-19 2018-08-21 东南大学 适于高维gnss/ins深耦合的容积卡尔曼滤波方法

Patent Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN105737828A (zh) * 2016-05-09 2016-07-06 郑州航空工业管理学院 一种基于强跟踪的相关熵扩展卡尔曼滤波的组合导航方法
CN113792412A (zh) * 2021-08-13 2021-12-14 中国人民解放军军事科学院国防科技创新研究院 一种基于中心误差熵准则容积卡尔曼滤波的航天器姿态确定方法
CN113792411A (zh) * 2021-08-13 2021-12-14 中国人民解放军军事科学院国防科技创新研究院 一种基于中心误差熵准则无迹卡尔曼滤波的航天器姿态确定方法

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
GNSS/INS紧组合最大熵卡尔曼滤波算法;李松等;《全球定位系统》;20200815(第04期);全文 *

Also Published As

Publication number Publication date
CN114858166A (zh) 2022-08-05

Similar Documents

Publication Publication Date Title
Wu et al. Generalized linear quaternion complementary filter for attitude estimation from multisensor observations: An optimization approach
CN111156987B (zh) 基于残差补偿多速率ckf的惯性/天文组合导航方法
CN109974714B (zh) 一种Sage-Husa自适应无迹卡尔曼滤波姿态数据融合方法
CN112013836B (zh) 一种基于改进自适应卡尔曼滤波的航姿参考系统算法
Del Rosario et al. Computationally efficient adaptive error-state Kalman filter for attitude estimation
Stovner et al. Attitude estimation by multiplicative exogenous Kalman filter
CN108562290B (zh) 导航数据的滤波方法、装置、计算机设备及存储介质
Dang et al. Cubature Kalman filter under minimum error entropy with fiducial points for INS/GPS integration
CN111983927A (zh) 一种新型的最大协熵椭球集员滤波方法
CN114858166B (zh) 基于最大相关熵卡尔曼滤波器的imu姿态解算方法
US20170074689A1 (en) Sensor Fusion Method for Determining Orientation of an Object
Gao et al. Random weighting-based nonlinear gaussian filtering
CN116007620A (zh) 一种组合导航滤波方法、系统、电子设备及存储介质
CN111649747A (zh) 一种基于imu的自适应ekf姿态测量改进方法
Pourtakdoust et al. An adaptive unscented Kalman filter for quaternion‐based orientation estimation in low‐cost AHRS
CN110375773B (zh) Mems惯导系统姿态初始化方法
CN110160530B (zh) 一种基于四元数的航天器姿态滤波方法
Fernandes et al. Gnss/mems-ins integration for drone navigation using ekf on lie groups
CN116608859A (zh) 一种基于阈值处理的自适应无迹卡尔曼滤波的导航方法、存储介质和设备
Candan et al. Estimation of attitude using robust adaptive Kalman filter
Zhao et al. A comparison of nonlinear filtering approaches for in-motion alignment of SINS
CN109489689B (zh) 一种基于α-β滤波的星矢量测量误差在轨估计方法
Hao et al. Particle filter for INS in-motion alignment
CN115667845A (zh) 用于移动载体的导航辅助方法
Shou et al. Micro-satellite attitude determination: using Kalman filtering of magnetometer data approach

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