卡尔曼滤波器计算无人机姿态

参考资料：基于陀螺仪及加速度计信号融合的姿态角度测量

前言

在上一篇介绍卡尔曼滤波器的文章中说到，对于无人机的姿态计算，我们不能单独的用陀螺仪或者是加速度计来进行测量，而是需要将它们俩进行融合，那么这一讲我们就来看看如何用卡尔曼滤波器进行滤波融合。

卡尔曼滤波融合过程

首先建立系统的状态方程和测量方程：

式中，$\varphi$是系统的真实角度，$b$是陀螺仪的零点偏移量，$\omega_{gyro}$为包含固定偏差的陀螺仪输出角速度，$\varphi_{acce}$为加速度计经处理后得到的角度值，$w_g$为陀螺仪测量噪声，$w_a$为加速度计测量噪声，$w_g$和$w_a$相互独立，此处假设二者为满足正态分布的白色噪声，令$T_s$为系统采样周期，得到离散系统的状态方程和测量方程：

并且系统过程噪声协方差矩阵$Q$以及测量误差的协方差矩阵$R$为：

式中，q_acce和q_gyro分别是加速度计和陀螺仪测量的协方差，其数值代表卡尔曼滤波器对其传感器数据的信任程度，值越小，表明信任程度越高。在该系统中陀螺仪的值更为接近准确值，因此取q_gyro的值小于q_acce的值。

此时，对应上一篇所介绍的卡尔曼滤波器的计算过程，我们现在已经知道了矩阵$A=\begin{bmatrix}1&-T_s\\0&1\end{bmatrix}$， 矩阵$B=\begin{bmatrix}T_s\\0\end{bmatrix}$，接下来我们只要给定一个初始化的系统状态$\hat x_0$以及它的方差$P_0$，还有干扰的协方差$Q$和$R$就可以带入公式计算了，具体计算过程请参考上一篇。下图是参考资料里用到的初始化参数，可以做一个参考：