通过上一篇文章我们看到虽然APM的源码已经将轴和角分离了，但是对yaw的限制还不够，但是这样处理有什么作用呢？ 我用MATLAB移植了这个程序，从数字上可以直观的看出结果。
思路：设置相同的当前姿态和期望姿态，用直接四元数计算误差，用四元数计算倾斜分离后的误差，然后分别换算成轴角。 该轴角度是控制器接收到的错误。 下面我们来看看两组数据的区别。 （程序的最后一部分是单独针对z轴的matlab角度转化为弧度，但是默认是26度，下一次循环需要更新姿态才能生效，这部分没有考虑，所以这次比较保留了z-轴在 26 度以内）
实验：
为了直观起见，以欧拉角的形式设置以度为单位的姿态
当前姿势 [0,0,0]，期望姿势 [30,30,25]
误差以度为轴角形式
直接计算误差为[22.3280, 35.0479, 16.0488]，简称[完全错误]
倾斜分离误差为[16.9144,37.8010,16.7880]，简称[分离误差]
从数值上看，z轴的误差相差不大，主要是x轴和y轴的误差不同，但大同小异。
为了直观起见，我们画出相应的姿势，
绿色为当前姿态，与黑色NED坐标系重合，
红色是预期的态度，和蓝色的【Complete Error】重合，这个很好理解，因为【Complete Error】是整个错误，所以当前态度加上所有的态度，一定和预期态度完全重合。
粉色的是【Separation Error】。 因为是处理后的误差，不可能和预期的误差完全吻合。 但是，z 轴未对齐。 这有点出乎意料。 我本来以为倾斜分离的目标是z轴Alignment，然后x,y有点误差。
所以我也分析了中间过程。
先计算中间过程，也就是对齐z轴的过程，简称【倾斜误差】
[倾斜误差] 为 [19.8108, 37.1418, 6.7148]
可以看到【Tilt Error】确实可以对齐z轴，和我们想的一样。
然后计算【旋转误差】=【0.0000, 0.0000, 16.7880】,
结合这个【倾斜误差】和【旋转误差】，蓝色坐标系与红色期望姿态完全重合。 （这里的组合是先旋转再倾斜，因为我们计算的时候是先倾斜再旋转）
也就是说，两个分开的旋转没有误差，【倾斜误差】可以对准z轴，【旋转误差】可以偏航。
但是因为APM直接丢弃了【倾斜误差】的z轴数据，所以最终的误差甚至没有和z轴对齐。
Vector3f rotation;
thrust_vec_correction_quat.to_axis_angle(rotation);
att_diff_angle.x = rotation.x;
att_diff_angle.y = rotation.y;
yaw_vec_correction_quat.to_axis_angle(rotation);
att_diff_angle.z = rotation.z;
这种现象确实和我想象的不一样。
我看APM程序最大的两个疑惑
1、倾斜分离后偏航为什么不处理（偏航26度以内）
2、为什么直接放弃【倾斜误差】的z轴，是为了限制偏航吗？
现在看来这两个疑点和实验是一致的，因为没有限制yaw，所以【Complete Error】和【Separation Error】的yaw几乎是一样的，因为【Tilt Error】的z轴是丢弃了，所以最终连接的z轴没有对齐。
为了解开这个疑惑，又去看了下PX4的算法。 没想到PX4的算法更新了。 初步看了一下，又多了几分疑惑。
3.为什么说APM是四元数算法，但是在各种旋转矩阵，旋转轴角的过程中，很多转换都会引入新的误差，但是PX4matlab角度转化为弧度，最后用了一组四元数来旋转轴角, 并且每一次 All times 都归一化以减少错误。
下次我会对比PX4的算法，仔细分析具体的细节。
总结：APM算法感觉除了最后的轴角处理有点奇怪。 前面的流程思路都非常清晰易懂。 主要问题是没有处理yaw，舍弃了[tilt error]的z轴，所以导致 实验结果是这个算法有tilt separation，和没有的区别不大倾斜分离。
部分源码：
%NED坐标系 当前机体坐标系 期望机体坐标系 （度）
NED=[0 0 0];
Cur_angle=[0 0 0];
Des_angle=[30 30 25];
%机体坐标系转换为弧度
cur_angle_radian=[Cur_angle(1)*pi/180,Cur_angle(2)*pi/180,Cur_angle(3)*pi/180] ;
des_angle_radian=[Des_angle(1)*pi/180,Des_angle(2)*pi/180,Des_angle(3)*pi/180];
%在NED下 画出 机体坐标系
plot_rotate_NED([0,0,0],Cur_angle,['g','g','g'],'-',1)
plot_rotate_NED([0,0,0],Des_angle,['r','r','r'],'-',1)
%当前姿态 Ccur->N
cur_dcm= euler2dcm(cur_angle_radian(1),cur_angle_radian(2),cur_angle_radian(3));
att_from_quat=euler2qual( cur_angle_radian(1),cur_angle_radian(2),cur_angle_radian(3) );
%当前姿态的旋转矩阵 取出z轴列（z轴单位向量） Zcur(N)
cur_att_thrust_vec=cur_dcm*[0;0;1];
%期望姿态 Ctar->N
des_dcm = euler2dcm( des_angle_radian(1),des_angle_radian(2),des_angle_radian(3) );
att_to_quat=euler2qual( des_angle_radian(1),des_angle_radian(2),des_angle_radian(3) );
%期望姿态的旋转矩阵 取出z轴列（z轴单位向量） Ztar(N)
des_att_thrust_vec=des_dcm*[0;0;1];
%不使用倾转分离的思路，直接计算 目标姿态-初始姿态=总误差姿态
all_correction_quat = Q_multipli( Q_INV(att_from_quat),att_to_quat );
%%N系下 两个向量的**旋转误差**
thrust_vec_cross=cross(cur_att_thrust_vec,des_att_thrust_vec);
%z轴误差角度 弧度
thrust_vec_dot=acos(dot(cur_att_thrust_vec,des_att_thrust_vec));
thrust_vector_length=norm(thrust_vec_cross);
if( (thrust_vector_length==0) || (thrust_vec_dot==0))
thrust_vec_cross = [0,0,1];
thrust_vec_dot = 0.0;
else
thrust_vec_cross = thrust_vec_cross/thrust_vector_length;
end
%轴角 转 四元数 描述的是 目标转到初始的旋转 所以对应的旋转是 Qtb1->cb 转轴在N系
thrust_vec_correction_quat=axis_angle2Q(thrust_vec_cross,thrust_vec_dot)
%%旋转 Qtb1->cb 转到初始坐标系
thrust_vec_correction_quat=Q_multipli(Q_INV(att_from_quat),Q_multipli(thrust_vec_correction_quat,att_from_quat))
%%画出 初始姿态 加上**倾斜误差** 之后的姿态 结果是 z轴对齐了
plot_rotate_body([0,0,0],Cur_angle,quat2euler(thrust_vec_correction_quat)*57.3,['g','g','g'],'-.',2)
%%画出 初始姿态 加上总误差 之后的姿态 结果是 两个坐标系完全重合
plot_rotate_body([0,0,0],Cur_angle,quat2euler(all_correction_quat)*57.3,['b','b','b'],'--',2)
%Qcb->tb1 * Qn->cb * Qtb->n = Qtb->tb1 (即**旋转误差**)
yaw_vec_correction_quat = Q_multipli(Q_INV(thrust_vec_correction_quat),Q_multipli(Q_INV(att_from_quat),att_to_quat));
all_axis_angle=Q2axis_angle(all_correction_quat);
tilt_axis_angle=Q2axis_angle(thrust_vec_correction_quat);
torsion_axis_angle=Q2axis_angle(yaw_vec_correction_quat);
tilt_torsion_angle=[tilt_axis_angle(1),tilt_axis_angle(2),torsion_axis_angle(3)]*57.3
att_diff_angle_x = tilt_axis_angle(1);
att_diff_angle_y = tilt_axis_angle(2);
att_diff_angle_z = torsion_axis_angle(3);
%att_diff_angle_z 弧度 (0.46弧度 = 26度)
if(att_diff_angle_z > 0.46)
att_diff_angle_z = constrain_float(wrap_PI(att_diff_angle_z), -0.46, 0.46);
yaw_vec_correction_quat=Q_from_axis_angle([0,0,att_diff_angle_z]);
att_to_quat = Q_multipli( att_from_quat,Q_multipli(thrust_vec_correction_quat,yaw_vec_correction_quat));
end
这个结果是非常出乎意料的，所以非常希望有兴趣的朋友可以加我微信，多和我交流。 如果您有任何问题，请指正。
