DCM姿态估计算法分析
飞控算法 以下内容主要是阅读Direction Cosine Matrix IMU: Theory （by William Premerlani and Paul Bizard）写成的内容整理，只是用来自己看的，所以多有省略，详细的请参见原文。
英文原版在这里 www.21qa.net/upfiles/DCMDraft2.pdf
中文翻译版在这里 https://www.gitbook.com/book/nephen/direction-cosine-matrix-imu-theory/details
初学者建议读翻译版，虽然有很多翻译的地方比较生硬，但是可以大大降低阅读难度，看内容要紧，原版资料以后慢慢看。
姿态相关
姿态估计
姿态估计使用的是gyro，accel，compass，通过互补滤波实现。
流程如下：总的控制框图>
DCM控制框图
思路总结：
- 旋转矩阵定义飞机朝向，可由描述旋转运动的非线性微分方程积分得到。
- 数值积分引入数值误差。积分误差，有限时间步长和有限采样率；量化误差，数值精度有限。
- 破坏了正交性，导致倾斜。需要正交化，归一化。
- 需要使用GPS和加速度计修正陀螺仪偏移。
基本几何定义
坐标系定义
方向余弦矩阵
旋转矩阵
1.陀螺仪信号计算方向余弦
旋转运动学方程
进行数值积分得到下面
旋转矩阵
遇到第一个障碍：旋转向量（earth坐标系）和旋转角速度向量（gyro测得，body=plane）在同一个坐标系下面。我们想知道机体系在地球系下的方向。
解决方案：使用对称性，body看earth和earth看body是一致的，方向相反。转而求解地球系在机体系下的方向。
转换到earth坐标系的矩阵，符号经过了变换，叉乘顺序已经交换（利用叉乘，陀螺信号不变而改变叉乘顺序
转换为微分形式)
陀螺仪的漂移需要消除。这里代入的是修正后的角速度。思路：使用GPS修正yaw，使用accel修正roll pitch。
角速度表达
以上只是向量，需要表达成为矩阵R
后面的是单位矩阵+反对称矩阵
以上的操作会引入误差：数据截断误差、陀螺漂移误差、陀螺偏置误差、增益误差，接下来讨论消除误差。
重规范化（正交化）
计算误差不断带入，导致不再正交，需要重新正交化
计算误差
误差带入，XY互相修正，不单独分配到一个轴。
Z轴直接由X叉乘Y得到
调整大小为1。一种是计算大小的开方，但是这样要开方，很难算。考虑到接近1，就直接使用泰勒展开。
(3-内积)/2 *自身。
漂移消除
巧妙地使用向量叉积表示误差。代表一个旋转轴和一个旋转方向。
把这个量去修正旋转矩阵。
使用PID控制，这里只需要使用PI，就可以消除漂移。
GPS
GPS的问题：报告延迟。滤波，显得平滑。跟踪平滑和静态导航（忽略速度位置的突变）。
GPS航向矢量和 IMU滚转X轴在水平面上的投影 之间的旋转误差 是偏移数量的指示。
旋转修正就是 R矩阵X列和对地航向叉积的 Z分量。
计算yaw偏航
这个是叉积
xbp=（rxx，ryy，0），GPS_COG = （COGX，COGY，0）
xbp X GPS_COG = （rxxCOGY - rxyCOGX）k ，就是这个向量的大小
使用大的yaw修正权重的情况
- 初始偏航锁定，GPS无法提供偏航
- 风，风速被处理为gyro偏移
- 飞机垂直运动，GPS提供信息无效
加速度计
加速度计测量的是重力+离心力+直线推力。离心力好测量，但是直线不好测量。
直线加速度解决之道，飞机不是一直加速或者减速，时而加速时而减速，所以只要不实现高频动态响应，就可以。
加速度计测量角度，陀螺仪测量速率。
离心加速度=陀螺矢量X速度矢量
问题：在紧凑连续的转弯中，加速度计会饱和，然后带入错误的修正。所以要避免出现，又因为陀螺仪也会，所以可以通过陀螺仪去限制转动速度。
反馈控制器
然后把这个东西反馈回去
使用DCM实现控制和导航
- 想要控制俯仰角 >知道俯仰角 >俯仰角=横滚轴 点积 竖直轴（earth_Z在机体系的投影）
- 想要控制横滚角 >知道横滚角 >横滚角=俯仰轴 点积 竖直轴（earth_Z在机体系的投影）
- 想要导航 >知道当前相对于目标点的偏航姿态 > body_X轴 叉乘 目标点方向（水平可忽略z分量）
- 想要知道是否上下颠倒 >检测body_Z与竖直轴（earth_Z在机体系的投影）的点积符号。
- 想要知道绕earth_Z的转向速度。就是在机体系下 旋转速度w在竖直轴（earth_Z在机体系的投影）的投影，就是点积。
感悟
- 为什么不能只看别人的解析而不看原版论文？
- 因为那样很多信息都损失掉了，而原版的着这水平是最高的，他能够从本质上讲清楚一个问题。而分享的那个人只是懂得，说的好看而已，并不比原作者理解的更深刻。最最关键的是那个推理过程没了，仅仅告诉你为什么，这样是远远不够的。
- 这就是为什么看资料要看领域中最经典的，看博客要看水平最高的，而不要看那些一般的。（翻译版是可以看的，减轻认知难度，快速上手）
另外吐槽一下CSDN的编辑器实在是太难用了，chrome 和微软edge支持都有问题，还要换到ie上来。这些图片还要一个一个插入，实在是太麻烦了。