前提・実現したいこと
加速度センサーとジャイロセンサーから回転角度を取得したいので、Madgwick Filterを実装することにしました。
https://www.samba.org/tridge/UAV/madgwick_internal_report.pdf
その際、参考にしたサイトが以下です。
Madgwick Filterを読んでみた
このサイトではMatlabによるテストをしているのですが、Quaternionから回転角度を出す際に、quat2angleという関数を使っています。
このquat2angleという関数をUnity(C#)上で実装したいのです。
#自身が書いたコード
csharp
1using System.Collections; 2using System.Collections.Generic; 3using UnityEngine; 4 5public class IMUFilter { 6 const float deltat = 0.001f; // 時間感覚 7 const float gyroMeasError = 3.14159265358979f * (5.0f / 180.0f); // gyroscope measurement error in rad/s (shown as 5 deg/s) 8 private float beta = Mathf.Sqrt(3.0f / 4.0f) * gyroMeasError; 9 10 // Global system variables 11 float a_x, a_y, a_z; 12 float w_x, w_y, w_z; 13 public float SEq_1 = 1.0f, SEq_2 = 0.0f, SEq_3 = 0.0f, SEq_4 = 0.0f; 14 15 public void Update(float w_x, float w_y, float w_z, float a_x, float a_y, float a_z){ 16 // Local system variables 17 float norm; 18 float SEqDot_omega_1, SEqDot_omega_2, SEqDot_omega_3, SEqDot_omega_4; 19 float f_1, f_2, f_3; 20 float J_11or24, J_12or23, J_13or22, J_14or21, J_32, J_33; 21 float SEqHatDot_1, SEqHatDot_2, SEqHatDot_3, SEqHatDot_4; 22 23 // Axulirary variables to avoid 24 float halfSEq_1 = 0.5f * SEq_1; 25 float halfSEq_2 = 0.5f * SEq_2; 26 float halfSEq_3 = 0.5f * SEq_3; 27 float halfSEq_4 = 0.5f * SEq_4; 28 float twoSEq_1 = 2.0f * SEq_1; 29 float twoSEq_2 = 2.0f * SEq_2; 30 float twoSEq_3 = 2.0f * SEq_3; 31 32 // Normalise the accelerometer measurement 33 norm = Mathf.Sqrt(a_x * a_x + a_y * a_y + a_z * a_z); 34 a_x /= norm; 35 a_y /= norm; 36 a_z /= norm; 37 38 // Compute the objective function and Jacobian 39 f_1 = twoSEq_2 * SEq_4 - twoSEq_1 * SEq_3 - a_x; 40 f_2 = twoSEq_1 * SEq_2 + twoSEq_3 * SEq_4 - a_y; 41 f_3 = 1.0f - twoSEq_2 * SEq_2 - twoSEq_3 * SEq_3 - a_z; J_11or24 = twoSEq_3; 42 J_12or23 = 2.0f * SEq_4; 43 J_13or22 = twoSEq_1; 44 J_14or21 = twoSEq_2; 45 J_32 = 2.0f * J_14or21; 46 J_33 = 2.0f * J_11or24; 47 48 // Compute the gradient (matrix multiplication) 49 SEqHatDot_1 = J_14or21 * f_2 - J_11or24 * f_1; 50 SEqHatDot_2 = J_12or23 * f_1 + J_13or22 * f_2 - J_32 * f_3; 51 SEqHatDot_3 = J_12or23 * f_2 - J_33 * f_3 - J_13or22 * f_1; 52 SEqHatDot_4 = J_14or21 * f_1 + J_11or24 * f_2; 53 54 // Normalise the gradient 55 norm = Mathf.Sqrt(SEqHatDot_1 * SEqHatDot_1 + SEqHatDot_2 * SEqHatDot_2 + SEqHatDot_3 * SEqHatDot_3 + SEqHatDot_4 * SEqHatDot_4); 56 SEqHatDot_1 /= norm; 57 SEqHatDot_2 /= norm; 58 SEqHatDot_3 /= norm; 59 SEqHatDot_4 /= norm; 60 // Compute the quaternion derrivative measured by gyroscopes 61 SEqDot_omega_1 = -halfSEq_2 * w_x - halfSEq_3 * w_y - halfSEq_4 * w_z; 62 SEqDot_omega_2 = halfSEq_1 * w_x + halfSEq_3 * w_z - halfSEq_4 * w_y; 63 SEqDot_omega_3 = halfSEq_1 * w_y - halfSEq_2 * w_z + halfSEq_4 * w_x; 64 SEqDot_omega_4 = halfSEq_1 * w_z + halfSEq_2 * w_y - halfSEq_3 * w_x; 65 66 // Compute then integrate the estimated quaternion derrivative 67 SEq_1 += (SEqDot_omega_1 - (beta * SEqHatDot_1)) * deltat; 68 SEq_2 += (SEqDot_omega_2 - (beta * SEqHatDot_2)) * deltat; 69 SEq_3 += (SEqDot_omega_3 - (beta * SEqHatDot_3)) * deltat; 70 SEq_4 += (SEqDot_omega_4 - (beta * SEqHatDot_4)) * deltat; 71 72 // Normalise quaternion 73 norm = Mathf.Sqrt(SEq_1 * SEq_1 + SEq_2 * SEq_2 + SEq_3 * SEq_3 + SEq_4 * SEq_4); 74 SEq_1 /= norm; 75 SEq_2 /= norm; 76 SEq_3 /= norm; 77 SEq_4 /= norm; 78 } 79 80 private float rad2deg(float _rad){ 81 return _rad*Mathf.Rad2Deg; 82 } 83} 84
回答2件
あなたの回答
tips
プレビュー
バッドをするには、ログインかつ
こちらの条件を満たす必要があります。