陀螺仪传感器建模与卡尔曼姿态解算

陀螺仪传感器建模与卡尔曼姿态解算
说到姿态解算，目前用到最多的应该是“六轴加速度陀螺仪传感器+卡尔曼滤波算法”。网上的代码和卡尔曼滤波算法的教程很多，但大多数都是分开的两块，很少有将其结合在一起的。
本篇博客重点解释陀螺仪加速度计传感器在姿态解算方面如何建立模型，以及如何编写相应的代码。
一.模型建立
对于角度计算，我们有如下公式：

其中 为陀螺仪测得的角速度
由于现实中陀螺仪存在漂移，所以对上式进行修正如下

其中Gyro为陀螺仪静态漂移
由此我们建立状态方程：

状态量：

测量量：


二.卡尔曼5个公式