卡尔曼滤波器计算欧拉角

Stevn

有大佬用卡尔曼滤波器解算欧拉角吗，我自己用卡尔曼滤波器算pitch和roll，都 还挺稳的，为什么一把地磁传感器的数据加进去，姿态角就乱了啊，感觉应该是坐标轴不重合的问题，但是不管怎么改坐标轴都不行啊，
附上代码
[mw_shl_code=c,true]exroll = atan(six_axis.accel_y / sqrt(six_axis.accel_x * six_axis.accel_x + six_axis.accel_z * six_axis.accel_z)) * RAD_TO_DEG;
  expitch = atan2(-six_axis.accel_x , six_axis.accel_z) * RAD_TO_DEG;[/mw_shl_code]

[mw_shl_code=c,true] Bfy = tempMagZ * sin(rollAngle) + tempMagY * cos(rollAngle);
  Bfx = tempMagX * cos(pitchAngle) + tempMagY * sin(pitchAngle) * sin(rollAngle) - tempMagZ * sin(pitchAngle) * cos(rollAngle);
  exyaw = atan2(Bfy, Bfx) * RAD_TO_DEG;[/mw_shl_code]

还有就是MPU6050的z轴加速度，我的板子水平放置时，是+1g，但是看手册上规定的正方向，水平放置时不应该是-1g吗？
求解答

坐标轴

正点原子

帮顶