CN111272158B - 复杂磁扰动场景mems电子罗盘的动态方位角解算方法 - Google Patents

Abstract

本发明公开了复杂磁扰动场景MEMS电子罗盘的动态方位角解算方法，包括：获取多冗余传感器中加速度计、磁力计和陀螺仪的输出值；判断是否需要进行滤波；若需要进行滤波，构建扩展卡尔曼滤波器，输出加速度计、磁力计和陀螺仪的输出值的最优估值；若不需要进行滤波，直接输出多冗余传感器中加速度计、磁力计和陀螺仪的输出值；分别用倾角补偿算法、磁场比例角补偿算法和陀螺仪Z轴积分算法解算方位角；根据载体当前运动姿态进行数据融合，得到最优方位角。本发明提供的复杂磁扰动场景MEMS电子罗盘的动态方位角解算方法，使MEMS电子罗盘在磁扰动和动态条件下，自适应完成准确的方位角解算，提高定向精度；算法结构简单，收敛速度快。

Description

复杂磁扰动场景MEMS电子罗盘的动态方位角解算方法

技术领域

本申请涉及电子罗盘技术领域，尤其涉及复杂磁扰动场景MEMS电子罗盘的动态方位角解算方法。

背景技术

近年来微米纳米技术快速发展，由MEMS器件所组成的管道系统在探测、机械、交通、军事等各个领域都被广泛应用，特别在交通领域中使用MEMS电子罗盘和MEMS陀螺仪等器件进行定姿定向已成为发展趋势。在载体运动时，MEMS电子罗盘中的磁力计容易受到外接磁扰动的影响，将导致MEMS电子罗盘计算的方位角完全失真；MEMS电子罗盘中的加速度计在遇到突发的加减速时，并会产生大量的额外加速度，从而计算载体姿态的精度下降，进而影响方位角的精度；MEMS电子罗盘中的陀螺仪误差会随时间累计；并且，在动态方位角的计算中，无法使用由起始位置校准的误差模型推算出来的椭圆拟合算法和12位置校准算法进行解算。

发明内容

本发明的目的是要提供一种复杂磁扰动场景MEMS电子罗盘的动态方位角解算方法，可以解决上述现有技术问题中的一个或者多个。

根据本发明的一个方面，提供一种复杂磁扰动场景MEMS电子罗盘的动态方位角解算方法，包括以下步骤：

获取多冗余传感器中加速度计、磁力计和陀螺仪的输出值；

判断是否需要进行滤波；

若需要进行滤波，构建扩展卡尔曼滤波器，输出加速度计、磁力计和陀螺仪的输出值的最优估值；

若不需要进行滤波，则直接输出多冗余传感器中加速度计、磁力计和陀螺仪的输出值；

分别使用倾角补偿算法、磁场比例角补偿算法和陀螺仪Z轴积分算法解算方位角；

根据载体当前运动姿态进行数据融合，得到最优方位角。

在一些实施方式中，多冗余传感器由两组MEMS传感器正向贴合构成，分别为第一传感器与第二传感器，第一传感器包括第一陀螺仪、第一磁力计和第一加速度计，第二传感器包括第二陀螺仪、第二磁力计和第二加速度计，且第一传感器与第二传感器关系如下所示：

式中，其中G1_xyz和G2_xyz分别为第一陀螺仪和第二陀螺仪三轴输出值，A1_xyz和A2_xyz分别为第一加速度计和第二加速度计三轴输出值，M1_xyz和M2_xyz分别为第一磁力计和第二磁力计三轴输出值。

在一些实施方式中，判断是否需要进行滤波，包括以下步骤：

获取磁力计的输出值计算所有时刻的磁场总量，划分时间区间，并以每个时间区间的前n个时刻的磁场总量的平均值作为基准，基准的磁场总量表达式如下：

式中，MF_n是第n时刻的磁场总量，MF_std代表基准的磁场总量；

计算每一个时间区间中磁场总量的最大值MF_{reg_max}和最小值MF_{reg_min}，并计算下列公式：

MF_{reg_var}＝MF_{reg_max}-MF_{reg_min}

式中，MF_{reg_var}代表每个区间的磁场波动数值，

代表每个区磁场环境与基准磁场环境之间的差异；

将所求MF_{reg_var}和

与预先设置的阈值相比较，若所求MF_{reg_var}和

均在所设阈值范围内，则判断不需要进行滤波；否则，则需要进行滤波。

在一些实施方式中，若需要进行滤波，构建扩展卡尔曼滤波器，输出加速度计、磁力计和陀螺仪的输出值的最优估值包括：

确定状态量如下：

式中，

和

分别是第一陀螺仪和第二陀螺仪三轴角速度状态值，

和

分别第一陀螺仪和第二陀螺仪的三轴加速度状态值，

和

是第一磁力计和第二磁力计的状态值。

确定状态方程如下：

其中，

计算式如下：

计算式如下：

计算式如下：

计算式如下：

计算式如下：

计算式如下：

式中，η_gxyz,η_wxyz,η_mxyz分别为三轴角速度状态误差、三轴角加速度状态误差、三轴磁力计状态误差，Δt为数据采样周期；

MM₁和MM₂分别是第一磁力计和第二磁力计的三轴的旋转矩阵，MM₁和MM₂的表达式分别如下：

ω_xyz是第一陀螺仪和第二陀螺仪提供的旋转矩阵，表达式如下：

用每一个状态量对状态方程进行偏微分，获得状态转移矩阵A：

确定观测量如下：

Z_n＝[G1_xyz G2_xyz M1_xyz M1_xyz]

确定观测方程如下：

式中，

分别为第一陀螺仪和第二陀螺仪观测噪声误差，

分别为第一磁力计和第二磁力计观测值噪声误差，H为观测转移矩阵，H的表达式如下：

卡尔曼滤波基本算法编排，该算法流程如下：

状态量更新方程：

x(k|k-1)＝A×x(k-1|k-1)+B×u(k)

均方误差方程：

P(k|k-1)＝A×P(k-1|k-1)×A′+Q

滤波增益矩阵：

K(k)＝P(k|k-1)×H′×(H×P(k|k-1)×H′+R)^-1

k时刻状态估值计算方程：

x(k|k)＝x(k|k-1)+K(k)×(Z(k)-H×x(k|k-1))

估计均方误差方程：

P(k|k)＝(I-K(k)×H)*P(k|k-1)

式中，状态误差协方差Q设置为0，以上公式不断循环运算，得到加速计、磁力计和陀螺仪的输出值的最优估值。

在一些实施方式中，倾角补偿算法具体包括以下步骤：

利用加速度计输出值计算载体的俯仰角和横滚角，公式如下：

式中，A_x、A_y和A_z分别为第一加速度计和第二加速度计测量值的平均值，α为俯仰角，β为横滚角；

将磁力计的测量值进行转化，求得磁力计输出值在水平面上X轴和Y轴的投影，转化公式如下：

式中，M_bx、M_by和M_bz分别为第一磁力计与第二磁力计测量值的平均值；

简化得到：

M_nx＝M_bx cosα+M_by sinαsinβ-M_bz sinαcosβ

M_ny＝M_by cosβ+M_bz cosβ

式中，M_nx为水平面上X轴的投影，M_ny为水平面上Y轴的投影；

计算方位角，公式如下：

在一些实施方式中，磁场比例角补偿算法具体包括以下步骤：

计算载体坐标系的X轴与Y轴切割磁力线的角度，公式如下：

式中，M_bx、M_by和M_bz分别为第一磁力计与第二磁力计测量值的平均值，θ为磁场X轴切割磁力线的角度，

为为磁场Y轴切割磁力线的角度；

将磁力计的测量值进行转化，求得磁力计输出值在水平面上X轴和Y轴的投影，转化公式如下：

简化得到：

式中，M′_nx为水平面上X轴的投影，M′_ny为水平面上Y轴的投影；

计算方位角，公式如下：

在一些实施方式中，陀螺仪Z轴积分算法具体包括以下步骤：

利用陀螺仪Z轴的角速度积分解算，公式如下：

式中，

为k时刻陀螺仪Z轴积分解算出来的方位角，

为k-1时刻陀螺仪Z轴积分解算出来的方位角，其中k≥1。

在一些实施方式中，根据载体当前运动姿态进行数据融合，得到最优方位角具体包括：

将倾角补偿算法、磁场比例角补偿算法和陀螺仪Z轴积分算法解算的方位角进行融合，具体公式如下：

为最优方位角，ε₁、ε₂和ε₃分别为倾角补偿算法、磁场比例角补偿算法和陀螺仪积分的权重系数，其中ε₁+ε₂+ε₃＝1。

本申请提供技术方案与现有技术相比，存在以下有益效果：

本发明通过构建多冗余传感器，控制电子罗盘的零点漂移，增强电子罗盘的抗干扰能力；使用扩展卡尔曼将电子罗盘的数据融合滤波，使MEMS电子罗盘在磁扰动和动态条件下，自适应完成准确的方位角解算，提高定向精度，提高了MEMS电子罗盘的使用范围；算法结构简单，收敛速度快。

另外，在本发明技术方案中，凡未作特别说明的，均可通过采用本领域中的常规手段来实现本技术方案。

附图说明

为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作一简单地介绍，显而易见地，下面描述中的附图是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。

图1是本申请一实施例提供的复杂磁扰动场景MEMS电子罗盘的动态方位角解算方法的流程图。

图2是本申请一实施例提供的MEMS电子罗盘多冗余结构轴向示意图。

图3是本申请实施例2中实验1中水平绕轴旋转方位角对比结果。

图4是本申请实施例2中实验1中俯仰50°绕轴旋转方位角对比结果。

图5是本申请实施例2中实验1中横滚50°绕轴旋转方位角对比结果。

图6是本申请实施例2中实验2中车载直线运动方位角对比结果。

具体实施方式

为使本发明实施例的目的、技术方案和优点更加清楚，下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

实施例1：

如图1所示为本发明一实施例提供的复杂磁扰动场景MEMS电子罗盘的动态方位角解算方法，包括以下步骤：

S11：获取多冗余传感器中加速度计、磁力计和陀螺仪的输出值；

S12：判断是否需要进行滤波；

S13：若需要进行滤波，构建扩展卡尔曼滤波器，输出加速度计、磁力计和陀螺仪的输出值的最优估值；

S14：若不需要进行滤波，则直接输出多冗余传感器中加速度计、磁力计和陀螺仪的输出值；

S15：分别使用倾角补偿算法、磁场比例角补偿算法和陀螺仪Z轴积分算法解算方位角；

S16：根据载体当前运动姿态进行数据融合，得到最优方位角。

在可选的实施方式中，多冗余传感器由两组MEMS传感器正向贴合构成，分别为第一传感器与第二传感器，第一传感器包括第一陀螺仪、第一磁力计和第一加速度计，第二传感器包括第二陀螺仪、第二磁力计和第二加速度计，且第一传感器与第二传感器关系如下所示：

式中，其中G1_xyz和G2_xyz分别为第一陀螺仪和第二陀螺仪三轴输出值，A1_xyz和A2_xyz分别为第一加速度计和第二加速度计三轴输出值，M1_xyz和M2_xyz分别为第一磁力计和第二磁力计三轴输出值。

使用两组MEMS传感器正向贴合构成多冗余传感器，目的是为了控制电子罗盘的零点漂移，增强电子罗盘的抗磁扰动能力，两组传感器正向方向安装，实现互补关系，轴向如图2所示。

在可选的实施方式中，判断是否需要进行滤波，包括以下步骤：

获取磁力计的输出值计算所有时刻的磁场总量，划分时间区间，并以每个时间区间的前n个时刻的磁场总量的平均值作为基准，基准的磁场总量表达式如下：

式中，MF_n是第n时刻的磁场总量，MF_std代表基准的磁场总量；

计算每一个时间区间中磁场总量的最大值MF_{reg_max}和最小值MF_{reg_min}，并计算下列公式：

MF_{reg_var}＝MF_{reg_max}-MF_{reg_min}

式中，MF_{reg_var}代表每个区间的磁场波动数值，MF_stdvar代表每个区磁场环境与基准磁场环境之间的差异；

将所求MF_{reg_var}和

与预先设置的阈值相比较，若所求MF_{reg_var}和

均在所设阈值范围内，则判断不需要进行滤波；否则，则需要进行滤波。

判断是否滤波是为了应对外界磁扰动自适应执行滤波。

在可选的实施方式中，若需要进行滤波，构建扩展卡尔曼滤波器，输出加速度计、磁力计和陀螺仪的输出值的最优估值包括：

确定状态量如下：

式中，

和

分别是第一陀螺仪和第二陀螺仪三轴角速度状态值，

和

分别第一陀螺仪和第二陀螺仪的三轴加速度状态值，

和

是第一磁力计和第二磁力计的状态值。

确定状态方程如下：

其中，

计算式如下：

计算式如下：

计算式如下：

计算式如下：

计算式如下：

计算式如下：

式中，η_gxyz,η_wxyz,η_mxyz分别为三轴角速度状态误差、三轴角加速度状态误差、三轴磁力计状态误差，Δt为数据采样周期；

MM₁和MM₂分别是第一磁力计和第二磁力计的三轴的旋转矩阵，MM₁和MM₂的表达式分别如下：

ω_xyz是第一陀螺仪和第二陀螺仪提供的旋转矩阵，表达式如下：

用每一个状态量对状态方程进行偏微分，获得状态转移矩阵A：

确定观测量如下：

Z_n＝[G1_xyz G2_xyz M1_xyz M1_xyz]

确定观测方程如下：

式中，

分别为第一陀螺仪和第二陀螺仪观测噪声误差，

分别为第一磁力计和第二磁力计观测值噪声误差，H为观测转移矩阵，H的表达式如下：

卡尔曼滤波基本算法编排，该算法流程如下：

状态估计方程：

x(k|k-1)＝A×x(k-1|k-1)+B×u(k)

上一时刻最优估计的误差协方差：

P(k|k-1)＝A×β(k-1|k-1)×A′+Q

当前时刻的卡尔曼增益：

K(k)＝P(k|k-1)×H′×(H×P(k|k-1)×H′+R)^-1

k时刻状态估值计算方程：

x(k|k)＝x(k|k-1)+K(k)×(Z(k)-H×x(k|k-1))

当前时刻最优估计的误差协方差：

P(k|k)＝(I-K(k)×H)*P(k|k-1)

式中，状态噪声协方差Q设置为0，以上公式不断循环运算，得到加速度计、磁力计和陀螺仪的输出值的最优估值。

在可选的实施方式中，倾角补偿算法具体包括以下步骤：

利用加速度计输出值计算载体的俯仰角和横滚角，公式如下：

式中，A_x、A_y和A_z分别为第一加速度计和第二加速度计测量值的平均值，α为俯仰角，β为横滚角；

将磁力计的测量值进行转化，求得磁力计输出值在水平面上X轴和Y轴的投影，转化公式如下：

式中，M_bx、M_by和M_bz分别为第一磁力计与第二磁力计测量值的平均值；

简化得到：

M_nx＝M_bxcosα+M_by sinαsinβ-M_bz sinαcosβ

M_ny＝M_by cosβ+M_bz cosβ

式中，M_nx为水平面上X轴的投影，M_ny为水平面上Y轴的投影；

计算方位角，公式如下：

在可选的实施方式中，磁场比例角补偿算法具体包括以下步骤：

计算载体坐标系的X轴与Y轴切割磁力线的角度，公式如下：

式中，M_bx、M_by和M_bz分别为第一磁力计与第二磁力计测量值的平均值，θ为磁场X轴切割磁力线的角度，

为为磁场Y轴切割磁力线的角度；

将磁力计的测量值进行转化，求得磁力计输出值在水平面上X轴和Y轴的投影，转化公式如下：

简化得到：

式中，M′_nx为水平面上X轴的投影，M′_ny为水平面上Y轴的投影；

计算方位角，公式如下：

在可选的实施方式中，陀螺仪Z轴积分算法具体包括以下步骤：

利用陀螺仪Z轴的角速度积分解算，公式如下：

式中，

为k时刻陀螺仪Z轴积分解算出来的方位角，

为k-1时刻陀螺仪Z轴积分解算出来的方位角，其中k≥1。

在可选的实施方式中，根据载体当前运动姿态进行数据融合，得到最优方位角具体包括：

将倾角补偿算法、磁场比例角补偿算法和陀螺仪Z轴积分算法解算的方位角进行融合，具体公式如下：

为最优方位角，ε₁、ε₂和ε₃分别为倾角补偿算法、磁场比例角补偿算法和陀螺仪积分的权重系数，其中ε₁+ε₂+ε₃＝1。

倾角补偿算法会受到额外加速度和磁扰动的影响，而磁场比例角补偿算法只会受到磁扰动的影响，陀螺仪Z轴积分算法会有随时间累积误差，把三种算法解算出来的方位角进行融合，可以进行互补，从而得到最优方位角。

本发明通过构建多冗余传感器，控制电子罗盘的零点漂移，增强电子罗盘的抗干扰能力；使用扩展卡尔曼将电子罗盘的数据融合滤波，使MEMS电子罗盘在磁扰动和动态条件下，自适应完成准确的方位角解算，提高定向精度，提高了MEMS电子罗盘的使用范围；算法结构简单，收敛速度快。

实施例2：

针对载体在复杂磁扰动环境下进行不同的运动状态，设计了两组实验，实验1为多冗余电子罗盘在三轴无磁转台上加入多种磁扰动进行绕轴旋转，实验2为多冗余电子罗盘安装在汽车上进行直线运动，数据均有由Labview上位机采集。

实验1将多冗余电子罗盘安装在三轴无磁转台的凹槽内，进行绕轴旋转，绕轴旋转过程为先顺时针旋转90°，再逆时针旋转90°，旋转过程中有停顿，并在旋转过程中施加磁扰动，包括智能手机发出的电磁波，通电中的导线和铁质材料。

实验1进行了3组对比实验，分别是水平绕轴旋转，俯仰50°绕轴旋转和横滚50°绕轴旋转，图3-图5为分别为水平绕轴旋转，俯仰50°绕轴旋转和横滚50°绕轴旋转的实验结果。

如图3-图5所示，实线代表载体运动的真实方位角，点划线代表复杂磁扰动动态方位角算法解算出来的方位角，虚线代表滤波前的电子罗盘的方位角。由实验结果可以得到，在不同倾角的绕轴旋转过程中，由于滤波前的电子罗盘方位角波动巨大，而经过本发明提出的复杂磁场扰动场景MEMS电子罗盘的动态方位角解算方法解算出来方位角十分稳定，并且能够在动态环境精准定向，精度能够保持在±1°以内。

实验2将多冗余电子罗盘安装在汽车顶部的架子上，选择的行驶路线为直线路段，由此可知真实的方位角保持不变，实验结果如图6所示。

其中，实线代表载体运动的真实方位角，由于是直线路段，所以真实的方位角是保持不变的，点划线是本发明所述方法解算出来的方位角，虚线代表滤波前的电子罗盘的方位角。在多冗余电子罗盘在车载直线行进过程中，遇到很多复杂磁扰动，从而导致方向角发生剧烈的波动，从而使方向角大幅度变化，导致定向失败。而经过复杂磁扰动动态方位角算法解算出来的方位角，受到磁扰动时，方位角并没有发生明显偏移，且整个直线行进过程中其方位角误差不超过1°，所以当外界磁场和加速度有剧烈变化时，通过本发明所述方法解算后的数据依然可以保持稳定。

以上所描述的装置实施例仅仅是示意性的，其中作为分离部件说明的单元可以是或者也可以不是物理上分开的，作为单元显示的部件可以是或者也可以不是物理单元，即可以位于一个地方，或者也可以分布到多个网络单元上。可以根据实际的需要选择其中的部分或者全部模块来实现本实施例方案的目的。本领域普通技术人员在不付出创造性的劳动的情况下，即可以理解并实施。

通过以上的实施方式的描述，本领域的技术人员可以清楚地了解到各实施方式可借助软件加必需的通用硬件平台的方式来实现，当然也可以通过硬件。基于这样的理解，上述技术方案本质上或者说对现有技术做出贡献的部分可以以软件产品的形式体现出来，该计算机软件产品可以存储在计算机可读存储介质中，如ROM/RAM、磁碟、光盘等，包括若干指令用以使得一台计算机设备(可以是个人计算机，服务器，或者网络设备等)执行各个实施例或者实施例的某些部分所述的方法。

最后应说明的是：以上实施例仅用以说明本发明的技术方案，而非对其限制；尽管参照前述实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本发明各实施例技术方案的精神和范围。

Claims (6)

1.复杂磁扰动场景MEMS电子罗盘的动态方位角解算方法，其特征在于，包括以下步骤：

获取多冗余传感器中加速度计、磁力计和陀螺仪的输出值；

判断是否需要进行滤波；

若需要进行滤波，构建扩展卡尔曼滤波器，输出加速度计、磁力计和陀螺仪的输出值的最优估值；

若不需要进行滤波，则直接输出多冗余传感器中加速度计、磁力计和陀螺仪的输出值；

分别使用倾角补偿算法、磁场比例角补偿算法和陀螺仪Z轴积分算法解算方位角；

根据载体当前运动姿态进行数据融合，得到最优方位角；

所述多冗余传感器由两组MEMS传感器正向贴合构成，分别为第一传感器与第二传感器，所述第一传感器包括第一陀螺仪、第一磁力计和第一加速度计，所述第二传感器包括第二陀螺仪、第二磁力计和第二加速度计，且所述第一传感器与第二传感器关系如下所示：

式中，其中G1_xyz和G2_xyz分别为第一陀螺仪和第二陀螺仪三轴输出值，A1_xyz和A2_xyz分别为第一加速度计和第二加速度计三轴输出值，M1_xyz和M2_xyz分别为第一磁力计和第二磁力计三轴输出值；

所述判断是否需要进行滤波，包括以下步骤：

获取磁力计的输出值计算所有时刻的磁场总量，划分时间区间，并以每个时间区间的前n个时刻的磁场总量的平均值作为基准，基准的磁场总量表达式如下：

式中，MF_n是第n时刻的磁场总量，MF_std代表基准的磁场总量；

计算每一个时间区间中磁场总量的最大值MF_{reg_max}和最小值MF_{reg_min}，并计算下列公式：

MF_{reg_var}＝MF_{reg_max}-MF_{reg_min}

式中，MF_{reg_var}代表每个区间的磁场波动数值，

代表每个区磁场环境与基准磁场环境之间的差异；

将所求MF_{reg_var}和

与预先设置的阈值相比较，若所求MF_{reg_var}和

均在所设阈值范围内，则判断不需要进行滤波；否则，则需要进行滤波。

2.根据权利要求1所述的复杂磁扰动场景MEMS电子罗盘的动态方位角解算方法，其特征在于，所述若需要进行滤波，构建扩展卡尔曼滤波器，输出加速度计、磁力计和陀螺仪的输出值的最优估值包括：

确定状态量如下：

式中，

和

分别是第一陀螺仪和第二陀螺仪三轴角速度状态值，

和

分别第一陀螺仪和第二陀螺仪的三轴加速度状态值，

和

是第一磁力计和第二磁力计的状态值；

确定状态方程如下：

其中，

计算式如下：

计算式如下：

计算式如下：

计算式如下：

计算式如下：

计算式如下：

式中，η_gxyz，η_wxyz，η_mxyz分别为三轴角速度状态误差、三轴角加速度状态误差、三轴磁力计状态误差，Δt为数据采样周期；

MM₁和MM₂分别是第一磁力计和第二磁力计的三轴的旋转矩阵，MM₁和MM₂的表达式分别如下：

ω_xyz是第一陀螺仪和第二陀螺仪提供的旋转矩阵，表达式如下：

用每一个状态量对状态方程进行偏微分，获得状态转移矩阵A：

确定观测量如下：

Z_n＝[G1_xyzG2_xyzM1_xyzM1_xyz]

确定观测方程如下：

式中，

分别为第一陀螺仪和第二陀螺仪观测噪声误差，

分别为第一磁力计和第二磁力计观测值噪声误差，H为观测转移矩阵，H的表达式如下：

卡尔曼滤波基本算法编排，该算法流程如下：

状态量更新方程：

x(k|k-1)＝A×x(k-1|k-1)+B×u(k)

均方误差方程：

P(k|k-1)＝A×P(k-1|k-1)×A′+Q

滤波增益矩阵：

K(k)＝P(k|k-1)×H′×(H×P(k|k-1)×H′+R)^-1

k时刻状态估值计算方程：

x(k|k)＝x(k|k-1)+K(k)×(Z(k)-H×x(k|k-1))

估计均方误差方程：

P(k|k)＝(I-K(k)×H)*P(k|k-1)

式中，状态误差协方差Q设置为0，以上公式不断循环运算，得到加速度计、磁力计和陀螺仪的输出值的最优估值。

3.根据权利要求2所述的复杂磁扰动场景MEMS电子罗盘的动态方位角解算方法，其特征在于，所述倾角补偿算法具体包括以下步骤：

利用加速度计输出值计算载体的俯仰角和横滚角，公式如下：

式中，A_x、A_y和A_z分别为第一加速度计和第二加速度计测量值的平均值，α为俯仰角，β为横滚角；

将磁力计的测量值进行转化，求得磁力计输出值在水平面上X轴和Y轴的投影，转化公式如下：

式中，M_bx、M_by和M_bz分别为第一磁力计与第二磁力计测量值的平均值；

简化得到：

M_nx＝M_bx cosα+M_by sinαsinβ-M_bz sinαcosβ

M_ny＝M_by cosβ+M_bz cosβ

式中，M_nx为水平面上X轴的投影，M_ny为水平面上Y轴的投影；

计算方位角，公式如下：

4.根据权利要求2所述的复杂磁扰动场景MEMS电子罗盘的动态方位角解算方法，其特征在于，磁场比例角补偿算法具体包括以下步骤：

计算载体坐标系的X轴与Y轴切割磁力线的角度，公式如下：

式中，M_bx、M_by和M_bz分别为第一磁力计与第二磁力计测量值的平均值，θ为磁场X轴切割磁力线的角度，

为磁场Y轴切割磁力线的角度；

将磁力计的测量值进行转化，求得磁力计输出值在水平面上X轴和Y轴的投影，转化公式如下：

简化得到：

式中，M′_nx为水平面上X轴的投影，M′_ny为水平面上Y轴的投影；

计算方位角，公式如下：

5.根据权利要求2所述的复杂磁扰动场景MEMS电子罗盘的动态方位角解算方法，其特征在于，所述陀螺仪Z轴积分算法具体包括以下步骤：

利用陀螺仪Z轴的角速度积分解算，公式如下：

式中，

为k时刻陀螺仪Z轴积分解算出来的方位角，

为k-1时刻陀螺仪Z轴积分解算出来的方位角，其中k≥1。

6.根据权利要求2所述的复杂磁扰动场景MEMS电子罗盘的动态方位角解算方法，其特征在于，所述根据载体当前运动姿态进行数据融合，得到最优方位角具体包括：

将倾角补偿算法、磁场比例角补偿算法和陀螺仪Z轴积分算法解算的方位角进行融合，具体公式如下：

为最优方位角，ε₁、ε₂和ε₃分别为倾角补偿算法、磁场比例角补偿算法和陀螺仪积分的权重系数，其中ε₁+ε₂+ε₃＝1。

Images