姿态解算中，为什么加速度计无法感知z轴上的旋转运动

刚好在看姿态解算部分，粗略回答一下，有答的不对的地方欢迎讨论交流。
互补滤波有两种方法，加速度计和陀螺仪 加速度计，陀螺仪和磁力计，也就是所谓的IMU和AHRS
IMU方案是用加速度计修正陀螺仪，关于加速度计求解角速度也即是把重力分解到机体系上，通过反三角函数可以求解角速度，对于无法感知z轴上的旋转运动可以这样理解，当机体沿着z轴方向旋转时，分解出来的向量与旋转前并没有变化，也即水平面上没有重力分量，所以加速度计无法感知z轴的旋转，所以如果用这种姿态结算方法时，需要手动将机头对准正北方向，所以偏航的求解是有误差的。

实现了基于四元数的姿态外环PID控制器（实际上是P+前馈）。
类的定义及实现存放在：mc_att_control/AttitudeControl
控制器输入为NED坐标系下四元数表示的当前姿态q和期望姿态qd，输出为机体坐标系下三轴的角速度。
如何由NED坐标系下表示的q和q_d得到在机体坐标系下的姿态误差是关键，本来这个误差由qinversed() qd就可以得到，但是考虑了优先控制roll和pitch，就有一些复杂，下面进行解释。
注：网上查到的这部分的源码解读大部分是旧版本的，本文基于V1101版本。

作用是给P参数赋值。
由于控制策略是优先控制roll和pitch，所以并不是直接把proportional_gain赋值给私有变量_proportional_gain。
具体方式为:

给控制器输出的角速度设置limit

PID控制器的实现。

将q和qd单位化

qdcm_z()得到的是将q转化为方向余弦矩阵，并取第三列形成的向量。
所以根据方向余弦的定义，e_z和e_z_d即为当前机体坐标系和期望机体坐标系的z轴在NED坐标系下的表示。

第三行调用了Quatf类的其中一种构造函数，qd_red代表的是从e_z旋转到e_z_d的四元数。
可以这样理解：当前姿态坐标系经过qd_red代表的旋转之后，两个坐标系（当前姿态坐标系和期望姿态坐标系）的z轴是对齐的。我们把这个z轴对齐的状态定义为一个中间状态。可以认为 qd_red包含了roll和pitch的误差信息 。
注意此时qd_red只是代表了一个旋转（q->qd），并不像q和qd代表了姿态（NED->q和NED->qd）。
由于于e_z和e_z_d是在NED坐标系下的向量，所以得到的qd_red也是在NED下表示的。

接下来是对一种极端情况的考虑

q_mix代表从中间状态qd_red到期望姿态qd的旋转四元数 在qd_red系 下的表示，可以表征yaw的误差。下面进行解释。

对q_mix规范化。
因为同一个旋转可以由互为相反数的两个四元数表示。为了避免奇异，进行统一表示。

由前面我们知道，q_mix代表从中间状态qd_red到期望姿态qd的旋转四元数在qd_red系下的表示，可以表征yaw的误差。，由于中间状态qd_red坐标系与期望姿态qd坐标系的z轴是重合的，所以q_mix在qd_red系下的表示应该是 ， 为旋转角，即只有q_mix(0)和q_mix(3)的值是我们需要的。

所以经过了_yaw_w参数作用后的，在反推回来得到的qd与最开始给定的qd以经有所变化，即最后算出来的roll和pitch的期望角速度相同，但计算出来的yaw的期望角速度根据_yaw_w的大小会有所减小，这样就实现了优先控制roll和pitch。

qe即为从q旋转到qd的的四元数在机体坐标系下的表示，即误差。

使用sin(alpha/2)缩放的旋转轴作为姿态误差(参见轴角定义的四元数)
还要考虑对映单元四元数的歧义性

得到期望的三轴角速度（用P参数乘以误差），并且加上yaw的前馈。
yawspeed_feedforward是绕NED系z轴旋转的前馈增益，但是我们需要在机体坐标系中应用它。
因此，我们通过取R的最后一列来推断出世界z轴(在body frame中表示)。将其取逆(== qinversed)并将其乘以偏航设定值率(yawspeed_feedforward)。
这就产生了一个在机体系下表示的前馈的向量。

限幅，返回期望角速度。

整个过程大致为：

这样一系列 *** 作，可以达到优先控制roll&pitch的效果。

有惯性坐标系 ，以及表示在 下的两个单位四元数 ，此时若存在 ，满足：


那么 有什么几何意义

初次接触，如有错误欢迎批评指正。

四元数多是用在中间环节，在最终的PID输出的时还是用的欧拉角。
我认为造成这个现象的根本原因是抄袭！！对，就是抄袭。
通过6轴或者9轴融合姿态角的算法有两种，这两种我记得没错的话是英国人开发的(国籍可能记错了)，并开源了，国内的那些没有研发能力的团体和个人就是用的这个算法，而那个算法的中间环节均为四元数，输出是欧拉角。
我也是没有研发能力的个人，所以也不能透彻理解这个转换的意义，猜测可能是像拉式变换或者傅立叶一样，通过一种域的变换使计算过程简便，也就是三维的'复数域'，再可能是通过四元数防止计算过程中出现超过欧拉角表示范围的问题。四元数在旋转合成方面会方便一些，计算量也少于欧拉角，但是！姿态算法里根本没有四元数的旋转合成。
国内的开源的四轴代码都是这样，还有以前看的一个开源代码开始还是开源的，后来还有限制了，就那破代码，也就开发者自己还觉得不错了。
这些代码没有核心的姿态算法的开发能力，就在传感器的数据上'下功夫'，就是被玩坏的滤波器，MWC用的滑动均值滤波、互补滤波简单明了，可靠有效，但是我们的开发者不这么认为，不用上椭圆滤波器，多阶低通滤波器，卡尔曼滤波这些复杂的算法就是低端的体现，浮点运算随心情写，也不管用不用的上，能用浮点绝不定点，这都还好，反正他们用的芯片完成他们的代码就是用大炮打蚊子，有的是资源没用，双精度都算的过来。
其中我最受不了的就是他们的卡尔曼滤波了，那个叫卡尔曼？天启者 卡尔玛吧。那都什么算法啊，有什么用，定义个QR就开始计算了，几个矩阵就高大上了，完全的低通性能，就是抑制dv/dt,确实给信号的波动减少了同时也把传感器的中高频信号给过滤掉了，这个可以，问题是中高频都滤掉了，控制频率给提到400+hz是什么意思，信号带宽给限制到了100hz，再把控制频率给升高，有什么用？有什么用？
说的这些都是我能看到源码的算法，当然有很多没有公开源码的好的算法，国内的牛人很多很多，只是他们专注于卖套件，谁都想把自己的劳动有回报，也不会公开，就是这个情况。
开源的国内代码就是这个质量，别想有什么突破，心态就是这样，别想短时间内有突破。
废话说了一堆也没解决你疑惑，实在抱歉。

---