（思路篇）姿态算解

(我们不谈武功，谈一谈想法)
什么是姿态：飞机在某个位置时，处于一个什么状态。因为飞机不是一个点，在同一个位置的飞机，有可能是不同的状态。

要控制飞机的姿态首先得描述出飞机的姿态
在飞机上建立一个机体坐标系Mb，地面上建立一个Mn (NED坐标系，北东地)，以地面上的视角描述飞机，其实就是相对于地面坐标系，机体坐标系处于一个什么状体。比如Mb的x轴，与Mn的x轴相差30度。建立的两个坐标系之后，我们就可以通俗易懂的描述飞机的姿态了。（具体理论可以看 《机器人学导论》 John J.Craig，我只分析思路）
描述有姿态几种方法。
最直接的就是旋转矩阵，飞机的姿态就是假设原来Mb与Mn是共原点完全重合的，然后机体坐标Mb绕原点做了一些旋转得到了Mb1。而旋转矩阵顾名思义就是用来描述旋转的，所以可以很直接的描述飞机的姿态。
（ 即Mb1*Cbn=Mn 旋转矩阵Cbn表示从b坐标系转换到n坐标系的旋转。）错误
Cbn*MB1=Mn,（一个坐标系中的向量，可以通过左乘一个旋转矩阵，转移到另一个坐标系，旋转矩阵的列是其中一个坐标系的单位向量在另一个坐标系的投影，所以之后用到的重力向量的投影是Cnb(地面向飞机投影)矩阵的第3列，但是我们给出的是Cbn,两者互为转置，所以在程序中对应Cbn的第3行）2016.6.21修改
上式表示当前Mb以一个Cbn形式的旋转得到了Mn。所以Cbn表示了飞机的姿态。
哈哈哈 原来描述姿态这么容易。接下来的思路理所应当是这样的，我期望飞机处于Cbn1状态，读取出当前姿态Cbn2，两者做差进入PID，姿态就被我控制了。
是的，基本思路就是这样的。但是万万没想到Cbn其实是这样的
先不考虑这个东西怎么得来的，意思是如果我能很容易的得到旋转矩阵，比较两个姿态的时候要比较9个数据咯，每次更新姿态要更新9个数据咯。而且这个矩阵好不直观啊，只看矩阵还是不懂飞机处于什么状态啊。
这个时候第二种描述方法欧拉角就站出来了。就是矩阵里的三角函数，φωψ，就是常说的pitch(俯仰),roll(横滚),yaw(偏航)，直接给你3个角度，你就可以比较直观的想象出飞机的姿态了。就用这个呀，这个好叼。
那么问题来了，我怎么得到这3个角度呢，好像飞机上没有这么牛的传感器啊！
到了最关键的时候大哥站出来了四元数，四元素是什么不重要，今天我们只谈思路，四元素可以解决我们之前的问题，如何得到这3个欧拉角？
哈哈哈，其实完整的公式是这样的，诶好像没感觉有多牛啊，还是不直观啊，我的欧拉角呢？

跟上图对应一下
C32=2(cb+ab),问题就变成如何求四元素abcd了


有了这个公式，你只需要知道3个轴的角速度（g.x,g.y,g.z）和周期时间(deltaT)就可以迅速得到abcd,然后得到pitch,roll,yaw.恰好飞机上就个核心传感器 叫陀螺仪，它能够直接得到三轴角速度。 perfect，完美解决问题。
如何算解姿态的问题解决了，那么我们面临下一个问题，滤波，因为所有传感都有误差，如何消除陀螺仪的误差呢？（未完待续，下次分解）

旋转矩阵是有旋转顺序的，不同的旋转顺序对应的Cbn的值不一样，也就是说你求欧拉角的过程中用到是数学公式是不一样的，但是求的结果是一样的。具体公式推导参考《机器人学导论》 John J.Craig[大神summer的博客，你为什么要跪着看博客？](http://blog.csdn.net/qq_21842557/article/details/50923863)[四元数解算姿态完全解析及资料汇总](http://wenku.baidu.com/link?url=zBZT8J9ufP41niCHBug8azL1gCO4qzOmEALCkiMv4XcuestIe0oDJI_kLnHP3cSemd5NE_yuUeqJnziPVYxO1c0bJi-SP0lZsNRUAl37VlW)[姿态解算理解](http://wenku.baidu.com/link?url=PMQf8b3bW1-D17Qme4tBv6oF28BEINX3YHGL114VpyzcFZIbIO5SIed_ibJwOApzQ6ij-dbmwZcuDkN7mwgtgAd-Spj9lZ5RV_NSJRgHk_e)