MPU6050卡尔曼滤波解算姿态角

前言

自己在课上吹的牛，课程作业再麻烦也得干。模了好几天鱼，终于在DDL前一天弄完了惯导模块的简单demo，卡尔曼滤波算是我弄的最久的了（大概2-3天），虽然没有彻底弄懂原理（概率论没学，隐马尔可夫链啥的更是不会），好歹就着教程推导给实现了。
实现的效果不咋的，还是存在明显的跳动，估计是MPU6050陀螺仪的事，方差大、数值基本是不准的，爬了。

理论推导

假定条件

使用陀螺仪和加速度计实现卡尔曼滤波有几个基本假设，我码字的时候忘了几个，先列出来，以后想起来再填。

1. 线性系统。对于稳态卡尔曼滤波，应该是线性时不变系统？
2. 静态条件，也就是只有重力加速度G的情况下，才能使用加速度推导出欧拉角。

欧拉角和四元数

这个部分其实只用了解一下欧拉角即可，但是作为一个开发备忘，绕任意轴旋转的矩阵、四元数还是要有的，万一以后用到了呢（狗头）。

欧拉角

我们这里说的欧拉角是Z-Y-X欧拉角，也就是先绕Z轴旋转α角，再绕Y轴旋转β角，最后绕X轴旋转γ角。示意图大概是这么个样子。一般Z轴取铅垂轴，X轴为物体主对称轴，Y轴为辅对称轴或用右手定则确定
那么我们就将绕Z轴旋转的角度Yaw称为偏航，绕X轴旋转的角度称为Roll滚转，绕Y轴旋转的角度Pitch称为俯仰角。

绕任意轴旋转的矩阵（Goldman公式）

欧拉角实际上是一种转角排列设定法，转角的排列顺序比较多，但是很多旋转实际上是一样的。我们可以找到一个等效的旋转轴和对应这个轴的转角，那么我们假定$$\hat{K}是一个表示旋转轴的单位向量，R_K(\theta )为等效旋转矩阵$$
那么我们可以将被旋转向量V分解为沿轴分量V_c和V_1，利用向量叉积取垂直V_c和V_1的V_2。设旋转后的向量为V’,其在平面分量为V’_1，有：$$\vec{V}=\overrightarrow{V_c}+\overrightarrow{V_1}$$
$$\overrightarrow{V^{{}^{\prime }}}=\overrightarrow{V_c}+\overrightarrow{V_1^{{}^{\prime }}}$$
示意图（意思意思就行了（狗头））
下面就是用V_1和V_2表示旋转后的向量投影，假设转角为θ，θ实际上就是V’_1和V_1的夹角（图上忘画了），那么旋转后的向量就是：
$$\overrightarrow{V^{’}}=\overrightarrow{V_c}+\overrightarrow{V_1}cos\theta +\overrightarrow{V_2}sin\theta$$
带入轴K的单位向量表示和各个坐标轴的坐标即可得到Goldman公式。例如：
$$\hat{K}=(K_x,K_y,K_z),\overrightarrow{V_c}=(\overrightarrow{V_c}\cdot \hat{K})\hat{K}$$
如果令V=（1，0，0），即X轴，那么代入旋转后向量公式就可以求得矩阵的第一列。
最后的旋转矩阵应该是：
[ K x 2 ( 1 − c ) + c K x K y ( 1 − c ) − K z s K x K z ( 1 − c ) − K y s