关于姿态融合算法的问题，算出的欧拉角为什么不对

来来回回折腾了很久，一直没搞定姿态融合算法，算出来的欧拉角怎么都不对。
说明：我用的传感器是mpu9150，定义的板子的坐标是：x轴指向北，y轴指向东，z轴指向地。陀螺仪、加速度计和磁力计的轴向经过旋转都保持这个朝向。
问题：我用的是AHRSupdate这个程序，但是算出来的欧拉角只有Roll是对的，Yaw和Pitch都不对，其中，当我把Roll旋转+90度时，Yaw的数据是对的，但是Pitch的怎么都不对，举个栗子：板子水平放置的时候Pitch保持为0，板子x轴倾斜+60度左右并保持此姿态的时候，Pitch只是在正负20之间来回震荡几次就又回到0。
我程序如下：
先初始化四元数，其中init_ax/init_ay/init_az的单位是g，比如z轴和重力方向重合时的读数是1；init_mx/init_my/init_mz的单位就是磁力计输出的单位，未经过任何改变：

1.   
   
2. init_Roll  = atan2(init_ay, init_az);
   
3. init_Pitch = -asin(init_ax);              //init_Pitch = asin(ay / 1);      
   
4. init_Yaw   = -atan2(init_my*cos(init_Roll) + init_mx*sin(init_Roll)*sin(init_Pitch) - init_mz*sin(init_Roll)*cos(init_Pitch),
   
5.                      init_mx*cos(init_Pitch) + init_mz*sin(init_Pitch));                       //atan2(my, mx);
   
6. q0 = cos(0.5*init_Roll)*cos(0.5*init_Pitch)*cos(0.5*init_Yaw) + sin(0.5*init_Roll)*sin(0.5*init_Pitch)*sin(0.5*init_Yaw);  //w
   
7. q1 = sin(0.5*init_Roll)*cos(0.5*init_Pitch)*cos(0.5*init_Yaw) - cos(0.5*init_Roll)*sin(0.5*init_Pitch)*sin(0.5*init_Yaw);  //x   
   
8. q2 = cos(0.5*init_Roll)*sin(0.5*init_Pitch)*cos(0.5*init_Yaw) + sin(0.5*init_Roll)*cos(0.5*init_Pitch)*sin(0.5*init_Yaw);  //y   
   
9. q3 = cos(0.5*init_Roll)*cos(0.5*init_Pitch)*sin(0.5*init_Yaw) - sin(0.5*init_Roll)*sin(0.5*init_Pitch)*cos(0.5*init_Yaw);  //z

复制代码

然后循环调用AHRSupdate()更新四元数，并用下面的公式结算欧拉角：

1. Pitch = asin(-2*q1*q3 + 2*q0*q2) * 57.3; //俯仰角，绕y轴转动         
   
2. Roll  = atan2(2*q2*q3 + 2*q0*q1,-2*q1*q1 - 2*q2*q2 + 1) * 57.3; //滚动角，绕x轴转动
   
3. Yaw   = atan2(2*q1*q2 + 2*q0*q3,-2*q2*q2 - 2*q3*q3 + 1) * 57.3;  //偏航角，绕z轴转动

复制代码

一直都找不到问题到底出在哪，感觉可能是我定义的坐标轴方向不能套用上面的公式来计算?可是为什么Roll的数据是对的呢?

初始角度是正確的嗎?

角度是正确的，举个例子：
板子放平，且x轴指向正北方时，初始化的q0=0.99xx   q1=-0.002xxx   q2=0.003xxx   q3=0.01xxxx

我想想看，AHRSupdate的话
用的是含有电子罗盘的版本吗?
有么有试过没有电子罗盘的?

AHRSupdata是含电子罗盘的。
IMUupdata是不含电子罗盘的，我之前试过，好像也不行

看程序感觉好像是电子罗盘没有校准的原因

用不带电子罗盘的IMUupdate程序试了一下，结果如下：
1. Pitch和Roll数据都对，只不过Pitch到不了90度，最多也就正负80度，这个在我的程序里应该算正常。
2. Yaw完全不受控制，它表示自己很随意。想是多少就是多少，看了下规律，是在-180到+180之间快速递减或递增，不像别人四轴飘的那么慢。
我的电子罗盘没有校准，不校准的话确实会差很多吗?应该不会导致Yaw和Pitch的数据都不对，而只有Roll的数据对这种现象吧。
改天校准一下试试，下周一给出测试结果和校准方法，以便大家参考和指出错误。

1. Pitch和Roll数据都对，只不过Pitch到不了90度，最多也就正负80度，这个在我的程序里应该算正常。
》加速计和陀螺仪有校正过吗?
2. Yaw完全不受控制，它表示自己很随意。想是多少就是多少，看了下规律，是在-180到+180之间快速递减或递增，不像别人四轴飘的那么慢。
》大部分四轴上都是直接用PID的D控制器的( Yaw = Kd*Gyro_Z ), 所以不会飘得很严重
3. 我的电子罗盘没有校准，不校准的话确实会差很多吗?
》这个要看周围环境的磁场干扰大不大，若没有干扰，影响不大，否则就有影响
4. 应该不会导致Yaw和Pitch的数据都不对，而只有Roll的数据对这种现象吧
》IMUupdate 的方法并没有用到电子罗盘, 所以若理论正确, Pitch , Roll 的误差来源应该是加速计或陀螺仪