Quaternionから回転角度を出す方法

前提・実現したいこと

加速度センサーとジャイロセンサーから回転角度を取得したいので、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