[发明专利]复杂磁扰动场景MEMS电子罗盘的动态方位角解算方法

权利要求书

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}和均在所设阈值范围内，则判断不需要进行滤波；否则，则需要进行滤波。