作为学控制的，多轴的姿态控制简单了解一些，当初看px4代码时，对其姿态控制算法表示不能理解，又很难找到一些资料详细解释，经过仔细的推导，大概对其有了一定的认识，因此记录下思路，大概解释下算法代码，如有错误，欢迎指出。
概述：px4的姿态控制，是根据SO3数学基础，推导旋转矩阵模型，采用串级PID控制，控制角度环和角速度环。实际上将姿态控制问题转换成平滑轨迹跟踪问题。
如图所示，该姿态控制主要关注如何平滑的转动。理论上来讲，该算法机动性强。
1.构建姿态设计旋转矩阵R_sp和当前姿态旋转矩阵R；
R_sp.set(_v_att_sp.R_body);
R.set(_v_att.R);
2.取出两个矩阵的Z轴向量，
math::Vector<3> R_z(R(0, 2), R(1, 2), R(2, 2));
math::Vector<3> R_sp_z(R_sp(0, 2), R_sp(1, 2), R_sp(2, 2));
根据这两个z轴计算出误差向量（参考坐标系），并转换到机体坐标系；
math::Vector<3> e_R = R.transposed() * (R_z % R_sp_z);
注：%已经符号重载过了，并不是求余，这里指的是向量叉积运算，后面的*，表示点积。
3.计算两个z轴的误差角度；
由公式a×b=︱a︱︱b︱sinθ，
a•b=︱a︱︱b︱cosθ，这里的
R_z和
R_sp_z又是单位向量，模为1，因此误差向量
e_R
的模就是
sinθ，点积就是
cosθ。
float e_R_z_sin = e_R.length();
float e_R_z_cos = R_z * R_sp_z;
4.计算yaw误差角度的权重；
yaw_w = R_sp(2, 2) * R_sp(2, 2);
5.因为多轴的yaw响应一般比roll/pitch慢一倍，因此将两者解耦，先补偿roll/pitch的变化，计算R_rp；
判定两个z轴是否存在误差if (e_R_z_sin > 0.0f)
将
e_R转成单位向量；
e_R = e_R_z_axis * e_R_z_angle;
计算
e_R_z_axis
的叉积矩阵，根据欧拉旋转公式，求出
R_rp；
R_rp = R * (_I + e_R_cp * e_R_z_sin + e_R_cp * e_R_cp * (1.0f - e_R_z_cos));
6.现在z轴都已经重合了，只需求yaw的误差角度；
取出两个矩阵的x轴（现在只有x轴存在误差）；
math::Vector<3> R_sp_x(R_sp(0, 0), R_sp(1, 0), R_sp(2, 0));
math::Vector<3> R_rp_x(R_rp(0, 0), R_rp(1, 0), R_rp(2, 0));
同样根据向量的叉积和点积求出误差角度的正弦和余弦，再反正切求出角度；
e_R(2) = atan2f((R_rp_x % R_sp_x) * R_sp_z, R_rp_x * R_sp_x) * yaw_w;
7.以上旋转方法，适用于小角度的误差，当转动的角度偏大时，还需另一种方法；
直接计算参考系到机体系的旋转矩阵，并转换成四元数形式；
q.from_dcm(R.transposed() * R_sp);
math::Vector<3> e_R_d = q.imag();
取出虚部，并归一化，根据
e_R_d *= 2.0f * atan2f(e_R_d.length(), q(0));
同样得到一个误差角度向量；
求出互补系数
direct_w = e_R_z_cos * e_R_z_cos * yaw_w;
更新e_R，包含两种旋转方法，互补；
e_R = e_R * (1.0f - direct_w) + e_R_d * direct_w;
8.求出角速度的期望值，供内环角速度控制使用；
_rates_sp = _params.att_p.emult(e_R);
9.yaw响应较慢，因此再加入一个前馈控制；
_rates_sp(2) += _v_att_sp.yaw_sp_move_rate * yaw_w * _params.yaw_ff;
上述用到的相关理论，读者自己查阅书籍。
参考资料：
pixhawk源码 mc_att_control；
High performance full attitude control of a quadrotor on SO(3)；
捷联惯性导航；