一种基于扩展卡尔曼滤波的无人机角加速度估计方法及装置

摘要

本发明涉及基于扩展卡尔曼滤波的无人机角加速度估计方法及装置，方法包括：S1初始化，S2预测过程，S3校正过程。本发明提供的基于扩展卡尔曼滤波的角加速度估计方法，从含有噪声的低成本传感器中估计出角加速度状态量，并且针对实际工程应用中的传感器不完全测量的问题和如何提取方位角，提供了计算策略。此外，本发明提供的基于扩展卡尔曼滤波的角加速度估计方法不仅可以估计出角加速度，还可以将陀螺仪，磁罗盘和加速度计原始值进行滤波。

主权项

一种基于扩展卡尔曼滤波的无人机角加速度估计方法，其特征在于，包括如下步骤：S1初始化，用以确定状态矢量和观测矢量；S2预测过程，根据上述状态矢量和观测矢量转化得到非线性动力学模型；基于非线性动力学模型进行初始估计，向前推算得到第一步的k+1先验状态估计：${\hat{X}}_{k+1}^{-}=f({\hat{X}}_k,0),$向前推算出误差协方差：其中，测量噪声矩阵Q_k保持变量σ²作为状态量对角项，用以作为滤波器的变量，S3校正过程，建立观测方程，计算卡尔曼增益：$K_k=P_k^{-}{H}_k^T{(H_k{P}_k^{-}{H}_k^T+R_k)}^{-1},$由观测变量z_k更新后验估计：${\hat{X}}_k={\hat{X}}_k^{-}+K_k(z_k-H_k{\hat{X}}_k^{-})$更新误差协方差：$P_k=(I-K_k{H}_k)P_k^{-}.,$继续步骤S2，进行迭代。