四轴姿态解算求解

redawnlc

并不是使用加速度计的本次测量值和陀螺仪的上次值做叉乘，而是使用上一时刻的姿态估计算出的重力值和当前时刻的量测值（加速度计输出）进行比较。叉乘可以利用sinθ表征出两个矢量不相关程度的大小，比如估计和量测结果完全重合的时候θ=0，叉乘乘积（误差）也为零。最后解算出误差之后用PID进行调节。