基于四元数和互补滤波对运动数据姿态解算整体思路版本（python）

1.姿态的描述----坐标系
为了建立运动物体运动姿态的数学模型，首先需要定义导航坐标系和载体坐标系。
导航坐标系n（O-X -Y -Z ）选取东-北-天建立坐标系，坐标原点为 O点，Y 轴正向指向北边，X 轴正向指向东边，
Z 轴正向指向天；载体坐标系 b（O-X -Y -Z ）选取右手直角坐标系，即向右为 X 轴正方向，向前为 Y 轴正方向，
向上为 Z 轴正方向。

2.四元数基础理论

3.互补滤波器原理
根据传感器之间的频域的互补特性，准备采用互补滤波算法对传感器的数据进行姿态角的求解。
陀螺仪动态响应特性好，但是在计算姿态时候，会有积分累计误差；而加速度计和磁力计不会有累计误差，但是他们动态响应特性差。
所以，三个传感器在频域上互补，故可以采用互补滤波器对传感器数据进行融合，进行姿态解算。
利用互补滤波器进行姿态信息数据融合中，将由加速度计和磁力计估算出的姿态信息通过低通滤波器与陀螺积分出的姿态进行进行融合。通过低通滤波器可以去除加速度信号和磁力计信号中夹杂的高频信号，与陀螺信号积分的姿态进行进行互补后可有效抑制陀螺漂移。

4.基于互补滤波姿态解算过程

具体过程如下：

(1)姿态估计初始，需要对四元数进行赋值
上面给出了，基于欧拉角对四元数的表示，具体的推导过程后面的文章会给出来，今天只讲一下整体的思路。
根据该公式，我们就可以确定四元数的初值。一般情况，理想状态下，我们认为刚开始欧拉角为0，所以，得到的此时的四元数为[1 0 0 0 ].当然这个初值也方便后面的计算。

(2) 数据预处理
为了消除原始噪声的干扰，进一步提高载体姿态估计精度。
在进行互补滤波姿态解算之前，对传感器原始数据进行滤波处理。将加速度计测得的载体坐标系下的加速度原始数据 A 经过 Butterworth 低通滤器后进行标准化，得到载体坐标
系下三轴加速度的分量够成的向量 为：

实际上，加速度计的校正只是把b 系和 n系的 XOY 平面重合起来，因此只能校
正俯仰角和横滚角，无法感知载体绕 Z 轴的旋转运动，即偏航角仍然存在角度误差，
因此需要磁力计来进一步进行校正。校正过程如下：
将磁力计测得数据进行归一化：

将磁力计数据从载体系转换到导航系，磁力计在导航系下的输出为

其中，mx,my,mz分别是磁力计传感器的实际测量值。
定义导航系中 X 轴指向正北方向。根据磁场原理，导航系下的 Z 轴方向分量相
等，且水平方向分量相等，由此得到磁力计在导航系下的理想输出为：

式中，bx,by,bz分别为磁力计在导航系下的理想输出在三个轴上的分量。
将导航下的磁力计的理想输出数据’转换到载体坐标系下，可得：

根据同一个向量在不同的坐标系下，向量的大小和方向理论上是一样的，我们将在载体坐标系下，把磁力计传感器的实际测量值和转换到载体坐标系下的磁力计的理想输出值，做叉乘运算，也叫向量积运算。

原因：
根据向量积公式：

在这里θ表示a和b之间的角度（0°≤θ≤180°），它位于这两个矢量所定义的平面上。而n是一个与a、b所在平面均垂直的单位矢量。
**对叉乘后的等式两边同时取模，我们就得到叉乘的结果是两个向量的夹角的正弦值，而在这个角度很小的时候，我们可以认为这个正弦值近似等于这个夹角。
**
所以说，叉乘后的结果近似看出两个向量的夹角了，那么，这个夹角有啥含义呢？
我们知道，加速度传感器可以得到横滚角和俯仰角，而磁力计传感器可以得到偏航角，那么，不难知道了，把加速度计和磁力计的理论输出和实际测量值，做个叉乘运算后，这个角度正是横滚角，俯仰角和偏航角的角度的误差！

下面分别是磁力计，加速度计的理论值和实际值的叉乘运算：


这样我们就得到了我们的误差，下面把这个误差求和后，加给陀螺仪传感器输出的角速度上，作为新的角速度值，通过四元数微分方程，进而求得四元数。

下图是新的角速度：
（因为陀螺仪随着时间会有累计误差，所以需要修正陀螺仪的误差)

对于这个误差，我们采用PID控制器或者PI控制器进行调节，

根据补偿后的角速度数据，带入四元数微分方程，求解四元数。

求解出来的四元数如下所示：

其中，T为传感器采集数据的周期。

最后一步，把四元数转换为欧拉角：

说明：欧拉角的旋转顺序有12种，常用的顺序是Z-Y-X。这个自己来确定，不同的旋转顺序对应的四元数和欧拉角的转换公式也不同，这个我还需要再看看。
好，今天先写到这里，整个流程就结束了。
后面会推到一些细节公式。