CN110470294A - 一种虚拟测量与卡尔曼滤波融合的载体姿态估计方法 - Google Patents

Abstract

本发明公开了一种虚拟测量与卡尔曼滤波融合的载体姿态估计方法，包括：虚拟测量阶段、卡尔曼滤波的预测阶段、更新阶段、虚拟测量阶段与卡尔曼滤波的预测阶段、更新阶段的融合和姿态估计；虚拟测量阶段与卡尔曼滤波的预测阶段、更新阶段的融合包括多次迭代方案和参数调整方案。为了验证所提方法的性能，我们进行了多次的转台实验。测试结果表明，与传统的基于卡尔曼滤波的方法相比，所提出的方法显著提高了姿态估计的实时性，具有较大的理论研究价值和工程实践意义。

Description

一种虚拟测量与卡尔曼滤波融合的载体姿态估计方法

技术领域

本发明属于姿态估计方法技术领域，具体涉及一种虚拟测量与卡尔曼滤波融合的载体姿态估计方法。

背景技术

载体姿态估计是导航定位领域的一大核心技术，具有广泛的应用场景，其中载体包括但不限于无人机、机器人、智能小车、行人等。

传统的姿态估计方法直接以陀螺采样输出作为输入，如欧拉角法通过求解欧拉角微分方程直接计算航向角、俯仰角和横滚角，欧拉角微分方程关系简单明了，概念直观，容易理解，解算过程无需做正交化处理，但方程中包含有三角运算，这给实时计算带来一定困难。方向余弦法是求解姿态矩阵微分方程，姿态矩阵微分方程实际上包含了九个未知量的线性微分方程组，存在6个多余的参数，计算量较大，不利于实时计算。传统姿态估计方法是按照某种数学表达式直接计算姿态，然而实际上载体姿态估计需要考虑更多的因素，包括环境噪声、载体运动状态，因此出现了先进的姿态估计方法。

先进的姿态估计方法包括两种方法：第一种方法基于互补滤波的方法，其利用加速度计、磁力计和陀螺仪在频域上的互补误差特性，利用梯度下降法或者PID方法(比例、积分、微分)进行姿态估计；第二种方法是基于卡尔曼滤波的方法，它是具有高斯噪声的线性动态***的最小方差状态估计，卡尔曼滤波的非线性版本是扩展卡尔曼滤波。

相比于姿态估计的精度问题，姿态估计的实时性问题越来越突出，尤其表现在以下三种情况：1)初始态度未知时姿态的快速收敛问题；2)载体的不同运动状态之间的突然切换；3)可能在短时间内导致较大姿态改变的其他场合。鉴于以上三种情况，有必要解决姿态估计的实时性问题。

通常，基于卡尔曼滤波的方法可通过调整***噪声协方差(Q)和量测噪声协方差(R)来改善载体姿态估计的实时性，如附图1所示，P表示误差协方差。一方面，当检测到状态估计不可靠，可以增加Q值；另一方面，如果检测到量测不可靠，可以增加R值。虽然基于卡尔曼滤波的方法很有吸引力，但是相关参数需要根据载体的运动状态以及环境噪声特性的变化而变化，这就导致参数调整过程复杂，一般方法得到的调整参数往往不可靠，限制了载体多运动状态下姿态估计实时性的提高。

因此现有的卡尔曼滤波方法在改善姿态估计的实时性方面还有很大的空间，为了解决上述问题，本发明提出了一种虚拟测量与卡尔曼滤波融合的载体姿态估计方法。

发明内容

本发明的目的在于提供一种实时性好的载体姿态估计方法，有效解决背景技术中存在的问题，对复杂运动条件下载体姿态估计的实时性问题和相关应用给出了指导。

为了实现上述目的，本发明采取的技术方案为：

一种虚拟测量与卡尔曼滤波融合的载体姿态估计方法，包括如下步骤：

虚拟测量阶段；

卡尔曼滤波的预测阶段；

卡尔曼滤波的更新阶段；

虚拟测量阶段与卡尔曼滤波的预测阶段、更新阶段的融合；

姿态估计；

其中，虚拟测量阶段是检测载体的运动状态，并估计虚拟测量的个数用于虚拟测量与预测阶段和更新阶段的融合，虚拟测量阶段包括如下步骤：

(1)MARG传感器校准；

(2)量纲归一化；

(3)模值计算；

(4)传感器相邻输出的叉乘；

(5)运动状态矩阵的合成；

(6)运动状态矩阵欧式距离的梯度；

(7)虚拟测量的个数估计。

进一步地，步骤(1)所述的MARG传感器的校准是校准加速度计、陀螺仪和磁力计的正交耦合误差、零偏误差和比例因子误差，校准公式如(1)、式(2)、式(3)所示，

其中，分别表示磁力计、陀螺仪和加速度计校准后的输出；m，ω和a分别表示磁力计三轴的原始输出，陀螺仪三轴的原始输出和加速度计三轴的原始输出；矩阵和分别表示磁力计、加速度计和陀螺仪的线性时不变误差，即比例因子误差和正交耦合误差；b_m、b_g和b_a分别表示磁力计、加速度计和陀螺仪的零偏误差；

步骤(2)所述的量纲归一化是将加速度除以9.8米/秒²、角速度除以1度/秒和磁场强度除以0.55Gs，这样消去MARG传感器数据的量纲，便于多源数据融合；

步骤(3)所述的模值计算是分别计算三轴加速度的模值、三轴角速度的模值和三轴磁场的模值，计算公式如(4)、式(5)、式(6)所示，

其中，和分别表示磁力计输出、陀螺仪输出和加速度计输出的模值； 分别表示磁力计X轴，Y轴和Z轴输出的平方；分别表示陀螺仪X轴，Y轴和Z轴输出的平方；分别表示加速度计X轴，Y轴和Z轴输出的平方；

步骤(4)所述的传感器相邻输出的叉乘是分别计算磁力计、陀螺仪和加速度计的当前时刻(k时刻)的输出与前一时刻(k-1时刻)输出的叉乘，计算公式如(7)、式(8)、式(9)所示，

相应的模值计算公式为：

其中，deltm、deltω、delta分别表示磁力计、陀螺仪和加速度计相邻输出的叉乘； 分别表示当前时刻(k时刻)磁力计、陀螺仪和加速度计的输出值； 分别表示前一时刻(k-1时刻)磁力计、陀螺仪和加速度计的输出值；

步骤(5)所述的运动状态矩阵的合成是指将步骤(3)中得到的三个模值和步骤(4)中得到的三个叉乘结果的模值合成一个6行1列或1行6列的运动状态矩阵，如式(10)或式(11)所示，该矩阵的模值表征了当前运动状态的复杂程度，如式(12)所示，

或

其中，ED表示运动状态矩阵；‖ED‖₂表示运动状态矩阵的模值；

步骤(6)所述的运动状态矩阵欧式距离的梯度是计算前后两个运动状态矩阵模值的比值，该比值为梯度，表征了载体运动状态的相对变化，计算公式如式(13)所示，

其中，Grad表示梯度；表示向上取整；‖ED^k‖₂、‖ED^k-1‖₂分别表示载体k时刻的运动状态矩阵的模值和载体k-1时刻运动状态矩阵的模值；

步骤(7)所述的虚拟测量个数的估计是利用分段函数的方法将步骤(6)中的梯度信息转化为虚拟测量的个数，如式(14)所示，

进一步地，所述卡尔曼滤波的预测阶段和卡尔曼滤波的更新阶段是基于卡尔曼率波方法实现的；所述卡尔曼滤波的预测阶段是根据状态误差协方差、***噪声和当前的状态量估计下一时刻的状态量，包括如下步骤：

状态预测：

误差协方差矩阵预测：

其中，表示k时刻状态量预测值；A表示状态矩阵；表示k-1时刻的状态量；B表示输入矩阵；μ_k表示***噪声向量；表示k时刻预测的误差协方差矩阵；表示k-1时刻的误差协方差矩阵；A^T表示状态矩阵的转置；Q表示***噪声协方差矩阵；

所述卡尔曼滤波的更新阶段是利用量测信息和量测噪声矩阵更正预测阶段得到的状态量，包括如下步骤：

卡尔曼增益计算：

状态更新：

误差协方差更新：

其中，表示k时刻的卡尔曼增益；H^T表示输出矩阵的转置；H表示输出矩阵；R表示测量噪声协方差矩阵；表示k时刻的状态量更新值；表示观测向量；表示更新后的误差协方差矩阵；I表示单位矩阵。

进一步地，所述虚拟测量阶段与卡尔曼滤波的预测阶段、更新阶段的融合包括多次迭代方案和参数调整方案；多次迭代方案和参数调整方案这两种具体的融合方案保证了对载体姿态估计的实时性；所述多次迭代方案是利用得到的虚拟测量的个数来控制预测阶段和更正阶段的迭代次数；所述参数调整方案是先进行预测阶段，然后利用得到的虚拟测量的个数调整状态误差协方差、***噪声和量测噪声，最后进行更新阶段，所述误差协方差、量测噪声、***噪声的参数调整如下：

进一步地，所述的姿态估计是将状态量转化为姿态角，所述姿态角包括俯仰角、横滚角和航向角，包括如下步骤：

其中，θ为俯仰角，为横滚角，Ψ为航向角，arcsin()为反正弦函数，arctan2()为四象限反正切函数，q₀,q₁,q₂,q₃为包含在状态量中的四元数，是工程上用来解算姿态角的数学工具。

本发明的有益效果：(1)本发明提出了虚拟测量阶段的具体操作步骤，给出了虚拟测量个数的估计方法；(2)首次提出了载体运动状态与卡尔曼滤波方法的融合方法，通过多次迭代方案或参数调整方案实现虚拟测量与卡尔曼滤波方法的融合；(3)相比于卡尔曼滤波方法，所提出的方法大大改善了载体在复杂运动状态，或者运动状态快速切换时姿态估计的实时性。

附图说明

图1为传统的基于卡尔曼滤波方法改善载体姿态估计实时性的示意图；

图2为本发明一种虚拟测量与卡尔曼滤波融合的载体姿态估计方法流程图；

图3为虚拟测量与卡尔曼滤波融合的方案为多次迭代方案时载体姿态估计方法的算法步骤图；

图4为虚拟测量与卡尔曼滤波融合的方案为参数调整方案时载体姿态估计方法的算法步骤图；

图5为载体在静态、变加速，匀加速切换时的数据输出的变化情况图(最大向心加速度8米/秒²)；

图6为载体在变加速、匀加速、变减速、静态切换时的数据输出的变化情况图(最大向心加速度15米/秒²)；

图7为载体在变加速、匀加速、变减速切换时的数据输出的变化情况图(最大向心加速度24米/秒²)。

具体实施方式

为了更好地理解本发明的内容，下面结合具体实施方法对本发明内容作进一步说明，但本发明的保护内容不局限以下实施例。

本发明旨在提出一种虚拟测量与卡尔曼滤波融合的载体姿态估计方法，该方法包括虚拟测量阶段的具体操作步骤以及虚拟测量个数的估计；通过多次迭代方案或参数调整方案实现虚拟测量与卡尔曼滤波方法的融合，提高了载体姿态估计的实时性。所提出的方法鲁棒性强、通用性好，具有较大的理论研究价值和工程实践意义。

实施例1

如图2所示，一种虚拟测量与卡尔曼滤波融合的载体姿态估计方法包括如下阶段：

虚拟测量阶段；

预测阶段；

更新阶段；

融合方案

姿态估计；

其中，融合方案包括多次迭代方案或参数调整方案。

实施例2

如图3所示，虚拟测量与卡尔曼滤波融合的方案为多次迭代方案。

虚拟测量阶段包括如下步骤：

MARG传感器校准：

所述的MARG传感器的校准是校准加速度计、陀螺仪和磁力计的正交耦合误差、零偏误差和比例因子误差，校准公式如(1)、式(2)、式(3)所示；

其中，分别表示磁力计、陀螺仪和加速度计校准后的输出；m，ω和a分别表示磁力计三轴的原始输出，陀螺仪三轴的原始输出和加速度计三轴的原始输出；矩阵和分别表示磁力计、加速度计和陀螺仪的线性时不变误差，即比例因子误差和正交耦合误差；b_m、b_g和b_a分别表示磁力计、加速度计和陀螺仪的零偏误差；

量纲归一化：

所述的量纲归一化是将加速度除以9.8米/秒²、角速度除以1度/秒和磁场强度除以0.55Gs，这样消去MARG传感器数据的量纲，便于多源数据融合。

模值计算：

所述的模值计算是分别计算三轴加速度的模值、三轴角速度的模值和三轴磁场的模值，计算公式如(4)、式(5)、式(6)所示，

其中，和分别表示磁力计输出、陀螺仪输出和加速度计输出的模值； 分别表示磁力计X轴，Y轴和Z轴输出的平方；分别表示陀螺仪X轴，Y轴和Z轴输出的平方；分别表示加速度计X轴，Y轴和Z轴输出的平方；

传感器相邻输出的叉乘：

所述的传感器相邻输出的叉乘是指分别计算加磁力计、陀螺仪和速度计的当前时刻(k时刻)的输出与前一时刻(k-1时刻)输出的叉乘，分别如式(7)、式(8)和式(9)所示。

相应的模值计算公式：

其中，deltm、deltω、delta分别表示磁力计、陀螺仪和加速度计相邻输出的叉乘； 分别表示当前时刻(k时刻)磁力计、陀螺仪和加速度计的输出值； 分别表示前一时刻(k-1时刻)磁力计、陀螺仪和加速度计的输出值；

运动状态矩阵的合成：

所述的运动状态矩阵的合成是指将模值计算步骤中得到的三个模值和传感器相邻输出的叉乘步骤中得到的三个叉乘结果的模值合成一个6行1列或1行6列的运动状态矩阵，如式(10)或式(11)所示。该矩阵的模值表征了当前运动状态的复杂程度，如式(12)所示。

或

其中，ED表示运动状态矩阵；‖ED‖₂表示运动状态矩阵的模值；

运动状态矩阵欧式距离的梯度：

所述的运动状态矩阵欧式距离的梯度是计算前后两个运动状态矩阵模值的比值，该比值为梯度，表征了载体运动状态的相对变化，如式(13)所示。

其中，Grad表示梯度；表示向上取整；‖ED^k‖₂、‖ED^k-1‖₂分别表示载体k时刻的运动状态矩阵的模值和载体k-1时刻运动状态矩阵的模值；

虚拟测量的个数估计：

所述的虚拟测量的个数估计是利用分段函数的方法将运动状态矩阵欧式距离的梯度信息转化为虚拟测量的个数，如式(14)所示。

虚拟测量个数来控制For循环的次数，从而决定For循环体中预测阶段和更新阶段的迭代次数。

预测阶段包括如下步骤：

状态预测：

误差协方差矩阵预测：

其中，表示k时刻状态量预测值；A表示状态矩阵；表示k-1时刻的状态量；B表示输入矩阵；μ_k表示***噪声向量；表示k时刻预测的误差协方差矩阵；表示k-1时刻的误差协方差矩阵；A^T表示状态矩阵的转置；Q表示***噪声协方差矩阵；

更新阶段包括如下步骤：

卡尔曼增益计算：

状态更新：

误差协方差更新：

其中，表示k时刻的卡尔曼增益；H^T表示输出矩阵的转置；H表示输出矩阵；R表示测量噪声协方差矩阵；表示k时刻的状态量更新值；表示观测向量；表示更新后的误差协方差矩阵；I表示单位矩阵。

姿态估计包括如下步骤：

其中，θ为俯仰角，为横滚角，Ψ为航向角，arcsin()为反正弦函数，arctan2()为四象限反正切函数，q₀,q₁,q₂,q₃为包含在状态量中的四元数，是工程上用来解算姿态角的数学工具。

实施例3

如图4所示，虚拟测量与卡尔曼滤波融合的方案为参数调整方案。

预测阶段包括如下步骤：

状态预测：

误差协方差矩阵预测：

其中，表示k时刻状态量预测值；A表示状态矩阵；表示k-1时刻的状态量；B表示输入矩阵；μ_k表示***噪声向量；表示k时刻预测的误差协方差矩阵；表示k-1时刻的误差协方差矩阵；A^T表示状态矩阵的转置；Q表示***噪声协方差矩阵；

虚拟测量阶段包括如下步骤：

MARG传感器校准：

所述的MARG传感器的校准是校准加速度计、陀螺仪和磁力计的正交耦合误差、零偏误差和比例因子误差，校准公式如(1)、式(2)、式(3)所示；

其中，分别表示磁力计、陀螺仪和加速度计校准后的输出；m，ω和a分别表示磁力计三轴的原始输出，陀螺仪三轴的原始输出和加速度计三轴的原始输出；矩阵和分别表示磁力计、加速度计和陀螺仪的线性时不变误差，即比例因子误差和正交耦合误差；b_m、b_g和b_a分别表示磁力计、加速度计和陀螺仪的零偏误差；

量纲归一化：

所述的量纲归一化是将加速度除以9.8米/秒²、角速度除以1度/秒和磁场强度除以0.55Gs,这样消去MARG传感器数据的量纲，便于多源数据融合。

模值计算：

所述的模值计算是分别计算三轴加速度的模值、三轴角速度的模值和三轴磁场的模值，计算公式如(4)、式(5)、式(6)所示，

其中，和分别表示磁力计输出、陀螺仪输出和加速度计输出的模值； 分别表示磁力计X轴，Y轴和Z轴输出的平方；分别表示陀螺仪X轴，Y轴和Z轴输出的平方；分别表示加速度计X轴，Y轴和Z轴输出的平方；

传感器相邻输出的叉乘：

所述的传感器相邻输出的叉乘是指分别计算磁力计、陀螺仪和加速度计的当前时刻(k时刻)的输出与前一时刻(k-1时刻)输出的叉乘，分别如式(7)、式(8)和式(9)所示。

相应的模值计算公式：

其中，deltm、deltω、delta分别表示磁力计、陀螺仪和加速度计相邻输出的叉乘； 分别表示当前时刻(k时刻)磁力计、陀螺仪和加速度计的输出值； 分别表示前一时刻(k-1时刻)磁力计、陀螺仪和加速度计的输出值；

运动状态矩阵的合成：

所述的运动状态矩阵的合成是指将模值计算步骤中得到的三个模值和传感器相邻输出的叉乘步骤中得到的三个叉乘结果的模值合成一个6行1列或1行6列的运动状态矩阵，如式(10)或式(11)所示。该矩阵的模值表征了当前运动状态的复杂程度，如式(12)所示。

或

其中，ED表示运动状态矩阵；‖ED‖₂表示运动状态矩阵的模值；

运动状态矩阵欧式距离的梯度：

所述的运动状态矩阵欧式距离的梯度是计算前后两个运动状态矩阵模值的比值，该比值为梯度，表征了载体运动状态的相对变化，如式(13)所示。

其中，Grad表示梯度；表示向上取整；‖ED^k‖₂、‖ED^k-1‖₂分别表示载体k时刻的运动状态矩阵的模值和载体k-1时刻运动状态矩阵的模值；

虚拟测量的个数估计：

所述的虚拟测量个数的估计是利用分段函数的方法将运动状态矩阵欧式距离的梯度信息转化为虚拟测量的个数，如式(14)所示。

误差协方差、量测噪声、***噪声等参数调整如下：

更新阶段包括如下步骤：

卡尔曼增益计算：

状态更新：

误差协方差更新：

其中，表示k时刻的卡尔曼增益；H^T表示输出矩阵的转置；H表示输出矩阵；R表示测量噪声协方差矩阵；表示k时刻的状态量更新值；表示观测向量；表示更新后的误差协方差矩阵；I表示单位矩阵。

姿态估计包括如下步骤：

其中，θ为俯仰角，为横滚角，Ψ为航向角，arcsin()为反正弦函数，arctan2()为四象限反正切函数，q₀,q₁,q₂,q₃为包含在状态量中的四元数，是工程上用来解算姿态角的数学工具。

实施例4

如图5所示，其中载体安装在半径为1.663米左右的转台上，最大向心加速度为8米/秒²，最大角速度为124.4度/秒，载体随转台一起，运动状态由静态切换到变加速，再由变加速切换到匀加速时3轴加速度(ax，ay，az)，3轴角速度(wx，wy，wz)以及3轴磁场(mx，my，mz)的变化情况。

实施例5

如图6所示，其中载体安装在半径为1.663米左右的转台上，最大向心加速度为15米/秒²，最大角速度为170.3度/秒，载体随转台一起，载体运动状态由变加速切换到匀加速，再由匀加速切换到变减速；再由变减速切换到静态时3轴加速度(ax，ay，az)，3轴角速度(wx，wy，wz)以及3轴磁场(mx，my，mz)的变化情况；。

实施例6

如图7所示，其中载体安装在半径为1.663米左右的转台上，最大向心加速度为24米/秒²，最大角速度为215.5度/秒，载体随转台一起，载体运动状态由变加速切换到匀加速，再由匀加速切换到变减速时3轴加速度(ax，ay，az)，3轴角速度(wx，wy，wz)以及3轴磁场(mx，my，mz)的变化情况。

实施例7

如表1所示，卡尔曼滤波、扩展卡尔曼滤波、以及本发明提出的姿态方法的实时性测试结果对比(延时均方根误差越小，实时性越好)。其中在实施例4中所示的几种状态变化中，用卡尔曼滤波方法进行姿态估计的延时均方根误差为35.2毫秒，扩展卡尔曼滤波方法的延时均方根误差为40.6毫秒，而本发明中所提出的方法延时均方根误差仅为10.2毫秒，远远低于前两种方法，实时性最好。类似地在实施例5中所示的几种状态变化中，用卡尔曼滤波方法进行姿态估计的延时均方根误差为61.2毫秒，扩展卡尔曼滤波方法的延时均方根误差为73.2毫秒，而本发明中所提出的方法延时均方根误差仅为9.8毫秒，远远低于前两种方法，实时性最好；类似地在实施例6中所示的几种状态变化中，用卡尔曼滤波方法进行姿态估计的延时均方根误差为80.2毫秒，扩展卡尔曼滤波方法的延时均方根误差为89.8毫秒，而本发明中所提出的方法延时均方根误差仅为10.6毫秒，远远低于前两种方法，实时性最好；在实施例4,5,6中，扩展卡尔曼滤波的延时均方根误差均大于卡尔曼滤波，这是因为扩展卡尔曼对***模型进行泰勒展开和一阶近似，导致计算量比卡尔曼滤波方法大；实施例4，5，6中载体的相对运动状态依次剧烈，因此同一种算法(卡尔曼滤波和扩展卡尔曼滤波)，在不同运动状态下的延时均方根误差不同(实时性不同)，且载体运动越剧烈，实时性越差；而本发明所提出的方法，通过融合虚拟测量量，大大改善了实时性，且实时性不随载体的运动状态的剧烈程度而变化。值得一提的是，本发明提出的方法估计姿态时仍有延时存在，其原因与环境噪声以及数据输出频率相关。

表1测试结果对比

以上所述仅为本发明的具体实施方式，不是全部的实施方式，本领域普通技术人员通过阅读本发明说明书而对本发明技术方案采取的任何等效的变换，均为本发明的权利要求所涵盖。

Claims (5)

1.一种虚拟测量与卡尔曼滤波融合的载体姿态估计方法，其特征在于，包括如下步骤：

虚拟测量阶段；

卡尔曼滤波的预测阶段；

卡尔曼滤波的更新阶段；

虚拟测量阶段与卡尔曼滤波的预测阶段、更新阶段的融合；

姿态估计；

其中，所述的虚拟测量阶段包括如下步骤：

(1)MARG传感器校准；

(2)量纲归一化；

(3)模值计算；

(4)传感器相邻输出的叉乘；

(5)运动状态矩阵的合成；

(6)运动状态矩阵欧式距离的梯度；

(7)虚拟测量的个数估计。

2.根据权利要求1所述的虚拟测量与卡尔曼滤波融合的载体姿态估计方法，其特征在于，步骤(1)所述的MARG传感器的校准是校准加速度计、陀螺仪和磁力计的正交耦合误差、零偏误差和比例因子误差，校准公式如(1)、式(2)、式(3)所示，

其中，分别表示磁力计、陀螺仪和加速度计校准后的输出；m，ω和a分别表示磁力计三轴的原始输出，陀螺仪三轴的原始输出和加速度计三轴的原始输出；矩阵和分别表示磁力计、加速度计和陀螺仪的线性时不变误差，即比例因子误差和正交耦合误差；b_m、b_g和b_a分别表示磁力计、加速度计和陀螺仪的零偏误差；

步骤(2)所述的量纲归一化是将加速度除以9.8米/秒²、角速度除以1度/秒和磁场强度除以0.55Gs；

步骤(3)所述的模值计算是分别计算三轴加速度的模值、三轴角速度的模值和三轴磁场的模值，计算公式如(4)、式(5)、式(6)所示，

其中，和分别表示磁力计输出、陀螺仪输出和加速度计输出的模值； 分别表示磁力计X轴，Y轴和Z轴输出的平方；分别表示陀螺仪X轴，Y轴和Z轴输出的平方；分别表示加速度计X轴，Y轴和Z轴输出的平方；

步骤(4)所述的传感器相邻输出的叉乘是分别计算磁力计、陀螺仪和加速度计的当前时刻的输出与前一时刻输出的叉乘，计算公式如(7)、式(8)、式(9)所示，

相应的模值计算公式为：

其中，deltm、deltω、delta分别表示磁力计、陀螺仪和加速度计相邻输出的叉乘； 分别表示当前时刻(k时刻)磁力计、陀螺仪和加速度计的输出值； 分别表示前一时刻(k-1时刻)磁力计、陀螺仪和加速度计的输出值；

步骤(5)所述的运动状态矩阵的合成是指将步骤(3)中得到的三个模值和步骤(4)中得到的三个叉乘结果的模值合成一个6行1列或1行6列的运动状态矩阵，如式(10)或式(11)所示，该矩阵的模值表征了当前运动状态的复杂程度，如式(12)所示，

或

其中，ED表示运动状态矩阵；‖ED‖₂表示运动状态矩阵的模值；

步骤(6)所述的运动状态矩阵欧式距离的梯度是计算前后两个运动状态矩阵模值的比值，该比值为梯度，表征了载体运动状态的相对变化，计算公式如式(13)所示，

其中，Grad表示梯度；表示向上取整；‖ED^k‖₂、‖ED^k-1‖₂分别表示载体k时刻的运动状态矩阵的模值和载体k-1时刻运动状态矩阵的模值；

步骤(7)所述的虚拟测量个数的估计是利用分段函数的方法将步骤(6)中的梯度信息转化为虚拟测量的个数，如式(14)所示，

3.根据权利要求1所述的虚拟测量与卡尔曼滤波融合载体姿态估计方法，其特征在于，所述卡尔曼滤波的预测阶段和卡尔曼滤波的更新阶段是基于卡尔曼率波方法实现的；所述卡尔曼滤波的预测阶段是根据状态误差协方差、***噪声和当前的状态量估计下一时刻的状态量，包括如下步骤：

状态预测：

误差协方差矩阵预测：

其中，表示k时刻状态量预测值；A表示状态矩阵；表示k-1时刻的状态量；B表示输入矩阵；μ_k表示***噪声向量；表示k时刻预测的误差协方差矩阵；表示k-1时刻的误差协方差矩阵；A^T表示状态矩阵的转置；Q表示***噪声协方差矩阵；

所述卡尔曼滤波的更新阶段是利用量测信息和量测噪声矩阵更正预测阶段得到的状态量，包括如下步骤：

卡尔曼增益计算：

状态更新：

误差协方差更新：

其中，表示k时刻的卡尔曼增益；H^T表示输出矩阵的转置；H表示输出矩阵；R表示测量噪声协方差矩阵；表示k时刻的状态量更新值；表示观测向量；表示更新后的误差协方差矩阵；I表示单位矩阵。

4.根据权利要求1所述的虚拟测量与卡尔曼滤波融合载体姿态估计方法，其特征在于，所述虚拟测量阶段与卡尔曼滤波的预测阶段、更新阶段的融合包括多次迭代方案和参数调整方案；所述多次迭代方案是利用得到的虚拟测量的个数来控制预测阶段和更正阶段的迭代次数；所述方差调整方案是先进行预测阶段，然后利用得到的虚拟测量的个数调整状态误差协方差、***噪声和量测噪声，最后进行更新阶段，所述误差协方差、量测噪声、***噪声的参数调整如下：

5.根据权利要求1所述的虚拟测量与卡尔曼滤波融合载体姿态估计方法，其特征在于，所述的姿态估计是将状态量转化为姿态角，所述姿态角包括俯仰角、横滚角和航向角，包括如下步骤：

其中，θ为俯仰角，为横滚角，Ψ为航向角，arcsin()为反正弦函数，arctan2()为四象限反正切函数，q₀,q₁,q₂,q₃为包含在状态量中的四元数，是工程上用来解算姿态角的数学工具。