基于最大相关熵卡尔曼滤波器的IMU姿态解算方法技术

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

【技术实现步骤摘要】
基于最大相关熵卡尔曼滤波器的IMU姿态解算方法


[0001]本专利技术属于惯性导航的姿态解算
，具体涉及一种基于最大相关熵卡尔曼滤波器的IMU姿态解算方法。

技术介绍

[0002]IMU（Inertial Measurement Unit，惯性测量单元）被广泛用于载体的姿态测量，一般由三轴的陀螺和三轴加速度计组成，特别是在智能机器领域，载体的运动跟踪和定位主要依靠IMU来确定。但是由于低成本的惯性器件会产生漂移、抗干扰的能力较差，导致精度很低。通常的做法是设计多传感器的融合算法，即引入磁力计，利用加速度计和磁力计的信息矫正陀螺仪的漂移，同时也利用动态下的陀螺输出修正加速度计结合磁力计的测量，消除磁场干扰等因素的影响。但是对于消除姿态解算过程中的非高斯噪声和未知干扰等方面的影响就非常有限。
[0003]MCC（Maximum Correntropy Criterion，最大相关熵准则）已经开始应用于卡尔曼滤波器中，对消除非高斯噪声和干扰有明显的优势。
[0004]利用MCKF（maximum correntropy Kalman filter，最大相关熵卡尔曼滤波器）进行IMU进行姿态解算，能够提升系统对于异常值和非高斯噪声的鲁棒性，从而进一步提升姿态解算的精度。

技术实现思路

[0005]鉴于上述的分析，本专利技术旨在公开了一种基于最大相关熵卡尔曼滤波器的IMU姿态解算方法，用于提升姿态解算的精度。
[0006]本专利技术公开了一种基于最大相关熵卡尔曼滤波器的IMU姿态解算方法，包括，建立两个随机变量的多核相关熵；所述多核相关熵为基于N次采样的包括p个高斯核的相关熵；建立IMU姿态解算的线性系统模型；对于所述线性系统模型，建立在多核相关熵下的最大相关熵卡尔曼滤波器；通过卡尔曼滤波进行IMU进行姿态解算。
[0007]进一步地，所述多核相关熵的表达式为：；其中，X,Y∈R
p
，为两个一维随机变量；N为采样次数；k=1,
…
N；为高斯函数，σ
i
为第i个元素的高斯核带宽，i=1,
…
p，p为高斯核的个数。
[0008]进一步地，建立IMU姿态解算的线性系统模型如下：；其中∈R
n
是IMU姿态解算的状态向量，∈R
m
是IMU姿态解算的观测向量，是状态转移矩阵，H是观测矩阵，q
k
和r
k
为假设彼此独立的白噪声；q
k
的协方差矩阵为；r
k
的协方差矩阵为。
[0009]进一步地，将建立IMU姿态解算的线性系统模型描述为：其中I∈R
n
×
n
是单位矩阵，x
k—
是x
k
的先验估计；噪声项ν
k
是：；噪声项ν
k
的协方差矩阵为；其中，是先验误差协方差，B
p
和B
r
为对和进行Cholesky分解获得；在两边左乘B
k
−1得到；其中，；ξ
k
为白噪声，E(ξ
k ξ
k
’
)=I。
[0010]进一步地，所述多核相关熵下的最大相关熵卡尔曼滤波器的MCC目标函数：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为观测向量的维度。
[0011]进一步地，在MCC下，最优估计量是：。
[0012]进一步地，满足最优解的状态向量；其中，；；；；σ
p
=[σ1,σ2,...,σ
n
]’
是过程带宽向量，σ
r
=[σ
n+1
,σ
n+2
,...,σ
n+m
]’
是测量带宽向量，e
p
是过程误差，e
r
是测量误差。
[0013]进一步地，通过卡尔曼滤波进行IMU进行姿态解算的过程包括：进行姿态解算的初始化；利用建立的IMU姿态解算的线性系统模型进行状态预测；利用预测的状态进行初始时刻的状态更新；进入卡尔曼滤波；根据更新的状态，判断是否满足滤波的约束条件；如果不满足，则执行卡尔曼滤波过程，对于滤波后的状态更新，继续判断是否满足滤波的约束条件；如果不满足继续执行卡尔曼滤波过程；如果满足，则进入到下一步；更新后验误差，输出解算结果。
[0014]进一步地，所述多核相关熵下的最大相关熵卡尔曼滤波器的滤波过程包括：；其中，为t时刻的状态更新值，为t
‑
1时刻的状态更新值，为状态的预测值，为t时刻的增益估计值，为先验误差协方差的估计值，为协方差矩阵的估计值，为M
p
的估计值，为M
r
的估计值，e
p
为过程误差，e
r
为测量误差。
[0015]进一步地，所述多核相关熵下的最大相关熵卡尔曼滤波器的约束条件为：；
其中，为t时刻的状态更新值，为t
‑
1时刻的状态更新值，为约束阈值，α和β为两个小的正数，在初始化时进行设置。
[0016]本专利技术至少可实现以下有益效果之一：本专利技术的基于最大相关熵卡尔曼滤波器的IMU姿态解算方法，增强惯性系统的鲁棒性，能够抑制系统姿态解算过程中的异常值、非高斯噪声和未知干扰；提升了系统的精度，能够可靠的估计陀螺的偏值，同时过滤来自加速度计和磁力计的输出异常值。
附图说明
[0017]附图仅用于示出具体实施例的目的，而并不认为是对本专利技术的限制，在整个附图中，相同的参考符号表示相同的部件：图1为本专利技术实施例中的基于最大相关熵卡尔曼滤波器的IMU姿态解算方法流程图；图2为本专利技术实施例中的MCKF算法流程图；图3为本专利技术实施例中的加入非高斯噪声后比较GD、标准姿态解算和MCKF三种算法解算的方位角对比图；图4为本专利技术实施例中的加入非高斯噪声后比较GD、标准姿态解算和MCKF三种算法解算的俯仰角对比图；图5为本专利技术实施例中的加入非高斯噪声后比较GD、标准姿态解算和MCKF三种算法解算的横滚角对比图；图6为本专利技术实施例中的加入非高斯噪声后比较GD、标准姿态解算和MCKF三种算法解算的三轴加速度计的输出对比图；图7为本专利技术实施例中的加入非高斯噪声后比较GD、标准姿态解算和MCKF三种算法解算的三轴陀螺仪的输出对比图；图8为本专利技术实施例中的加入非高斯噪声后比较GD、标准姿态解算和MCKF三种算法解算的三轴磁力计对比图。
具体实本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.一种基于最大相关熵卡尔曼滤波器的IMU姿态解算方法，其特征在于，包括，建立两个随机变量的多核相关熵；所述多核相关熵为基于N次采样的包括p个高斯核的相关熵；建立IMU姿态解算的线性系统模型；对于所述线性系统模型，建立在多核相关熵下的最大相关熵卡尔曼滤波器；通过卡尔曼滤波进行IMU进行姿态解算。2.根据权利要求1所述的IMU姿态解算方法，其特征在于，所述多核相关熵的表达式为：；其中，X,Y∈R
p
，为两个一维随机变量；N为采样次数；k=1,
…
N；为高斯函数，σ
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次采样时的采样值。3.根据权利要求1所述的IMU姿态解算方法，其特征在于，建立IMU姿态解算的线性系统模型如下：；其中∈R
n
是IMU姿态解算的状态向量，∈R
m
是IMU姿态解算的观测向量，是状态转移矩阵，H是观测矩阵，q
k
和r
k
为假设彼此独立的白噪声；q
k
的协方差矩阵为；r
k
的协方差矩阵为。4.根据权利要求3所述的IMU姿态解算方法，其特征在于，将建立IMU姿态解算的线性系统模型描述为：其中I∈R
n
×
n
是单位矩阵，x
k—
是x
k
的先验估计；噪声项ν
k
是：；噪声项ν
k
的协方差矩阵为；其中，是先验误差协方差，B
p
和B
r
为对和进行Cholesky分解获得；在两边左乘B
k
−1得到；
其中，；ξ
k
为白噪声，E(ξ
k
ξ
k
’
)=I。5.根据权利要求4所述的IMU姿态解算方法，其特征在于，所述多核相关熵下的最大相关熵卡尔曼滤波器的MCC目标函数：G
σi
(e
i,k
)的...

【专利技术属性】
技术研发人员：付博，
申请(专利权)人：北京神导科技股份有限公司，
类型：发明
国别省市：