一种基于自适应扩展卡尔曼滤波的AHRS算法

摘要

本发明涉及导航定姿领域，尤其涉及姿态航向参考系统AHRS(attitude and heading reference system)中通过采用自适应卡尔曼滤波，解决不同动态下卡尔曼滤波观测噪声矩阵的确定问题，以及通过扩展卡尔曼滤波解决高动态场景下由于传统卡尔曼滤波线性化引入的误差，从而提高低成本AHRS的姿态测量精度。

主权项

一种基于自适应扩展卡尔曼滤波的AHRS算法，其特征在于，包括以下步骤：步骤一：将AHRS系统保持静止，分别记录AHRS系统中加速度计的输出和陀螺的输出；根据加速度计的输出计算加速度计的零偏噪声参数，根据陀螺的输出分别计算陀螺的零偏噪声参数和陀螺的零偏；步骤二：根据加速度计的零偏噪声参数设置卡尔曼滤波量测信息的初始噪声矩阵，根据陀螺的零偏噪声参数设置卡尔曼滤波的初始噪声协方差矩阵；并根据陀螺的零偏设置卡尔曼滤波的初始状态向量；步骤三：根据陀螺的输出构建卡尔曼滤波的状态转移矩阵，根据构建的状态转移矩阵、初始状态向量以及初始噪声协方差矩阵预测卡尔曼滤波本次历元的状态向量和本次历元的噪声协方差矩阵；步骤四：构建卡尔曼滤波的量测方程矩阵，根据加速度计的输出和初始噪声矩阵计算本次历元的噪声矩阵，根据量测方程矩阵和本次历元的噪声矩阵更新卡尔曼滤波本次历元的状态向量和本次历元的噪声协方差矩阵；步骤五：根据更新后的卡尔曼滤波本次历元的状态向量补偿陀螺的输出；将更新后的本次历元的状态向量、更新后的本次历元的噪声协方差矩阵和本次历元的噪声矩阵分别一一对应作为初始状态向量、初始噪声协方差矩阵和初始噪声矩阵，转入步骤三。