[发明专利]一种基于自适应扩展卡尔曼滤波的AHRS算法

权利要求书

1.一种基于自适应扩展卡尔曼滤波的AHRS算法，其特征在于，包括以下步骤：

步骤一：将AHRS系统保持静止，分别记录AHRS系统中加速度计的输出和陀螺的输出；根据加速度计的输出计算加速度计的零偏噪声参数，根据陀螺的输出分别计算陀螺的零偏噪声参数和陀螺的零偏；

步骤二：根据加速度计的零偏噪声参数设置卡尔曼滤波量测信息的初始噪声矩阵，根据陀螺的零偏噪声参数设置卡尔曼滤波的初始噪声协方差矩阵；并根据陀螺的零偏设置卡尔曼滤波的初始状态向量；

步骤三：根据陀螺的输出构建卡尔曼滤波的状态转移矩阵，根据构建的状态转移矩阵、初始状态向量以及初始噪声协方差矩阵预测卡尔曼滤波本次历元的状态向量和本次历元的噪声协方差矩阵；

步骤四：构建卡尔曼滤波的量测方程矩阵，根据加速度计的输出和初始噪声矩阵计算本次历元的噪声矩阵，根据量测方程矩阵和本次历元的噪声矩阵更新卡尔曼滤波本次历元的状态向量和本次历元的噪声协方差矩阵；

步骤五：根据更新后的卡尔曼滤波本次历元的状态向量补偿陀螺的输出；将更新后的本次历元的状态向量、更新后的本次历元的噪声协方差矩阵和本次历元的噪声矩阵分别一一对应作为初始状态向量、初始噪声协方差矩阵和初始噪声矩阵，转入步骤三。

2.根据权利要求1所述的一种基于自适应扩展卡尔曼滤波的AHRS算法，其特征在于，步骤一中根据加速度计的输出计算加速度计的零偏噪声参数，根据陀螺的输出分别计算陀螺的零偏噪声参数和陀螺的零偏，具体计算方法如下：

式中，N为在静止时间段内的历元数量，为加速度计x,y,z三轴的输出或者陀螺x,y,z三轴的输出，σ_bg²为陀螺的零偏噪声参数或加速度计的零偏噪声参数；

陀螺的零偏计算方法如下：

式中，b_gx0、b_gy0、b_gz0分别为陀螺x,y,z三轴的零偏，ωx_i、ωy_i、ωz_i为陀螺x,y,z三轴的输出。

3.根据权利要求1所述的一种基于自适应扩展卡尔曼滤波的AHRS算法，其特征在于，步骤二中根据陀螺的零偏噪声参数设置卡尔曼滤波的初始噪声协方差矩阵并根据陀螺的零偏设置卡尔曼滤波的初始状态向量具体为：

卡尔曼滤波的状态向量X表示为：

X＝[q₀ q_x q_y q_z b_gx b_gy b_gz]^T；

根据陀螺的零偏设置卡尔曼滤波的初始状态向量X₀：

X₀＝[1 0 0 0 b_gx0 b_gy0 b_gz0]^T；

式中，b_gx0、b_gy0、b_gz0为陀螺x,y,z轴的零偏，q₀、q_x、q_y、q_z为所设定的四元数；

设置卡尔曼滤波方程的初始噪声协方差矩阵Q₀：

其中为陀螺x,y,z轴的零偏噪声参数，为四元数的噪声。