私は私IMUのプロジェクトでさまざまな方法を実装しており、現在、有名なMadgwickによるMayhonyのAHRSアルゴリズムの実装(MahonyAHRS.c)を試しています。
これまでのところ、ベクトルの回転と実行中の平均センサーフュージョン (補完と呼ばれることが多い) に基づいた自己実装のものと同じくらいうまく機能します。
ただし、360 度の自由度を取得したいと考えており、現在この機能を使用しています。
void imu_mahony_getRollPitchYaw(float *roll_, float *pitch_, float *yaw_)
{
*yaw_ = fast_atan2f(2*q1*q2 - 2*q0*q3, 2*q0*q0 + 2*q1*q1 - 1);
*pitch_ = -asin(2*q1*q3 + 2*q0*q2);
*roll_ = atan2f(2*q2*q3 - 2*q0*q1, 2*q0*q0 + 2*q3*q3 - 1);
}
ただし、その結果、90 度の問題が発生します。私が望むのは、ピッチとロールを 0 ～ 359 度の形で受け取ることです。