[kaze's test] プログラミングメモ
9軸センサーMahonyAHRSアルゴリズム（マホーニーAHRS）
Mahony AHRS

→目次

ネット上で有名な9軸センサーMahonyAHRSアルゴリズムのソースコードMahonyAHRS.cを読んだので注釈を付けました。クォータニオン（四元数）などの知識を事前に軽く勉強すれば、早く理解できるかもしれません。

1	//=====================================================================================================
2	// MahonyAHRS.c
3	//=====================================================================================================
4	//
5	// Madgwick's implementation of Mayhony's AHRS algorithm.
6	// See: http://www.x-io.co.uk/node/8#open_source_ahrs_and_imu_algorithms
7	//
8	// Date            Author            Notes
9	// 29/09/2011    SOH Madgwick    Initial release
10	// 02/10/2011    SOH Madgwick    Optimised for reduced CPU load
11	//
12	//=====================================================================================================
13	
14	//---------------------------------------------------------------------------------------------------
15	// Header files
16	
17	#include "MahonyAHRS.h"
18	#include < math.h >
19

定数宣言：サンプル周波数, 積分制御係数, 比例制御係数 
20	//---------------------------------------------------------------------------------------------------
21	// Definitions
22	
23	#define sampleFreq    512.0f         // sample frequency in Hz
24	#define twoKpDef    (2.0f * 0.5f)    // 2 * proportional gain
25	#define twoKiDef    (2.0f * 0.0f)    // 2 * integral gain
26
変数宣言: 積分制御係数, 比例制御係数, 姿勢のクォータニオン（四元数）宣言と初期化, 積分制御の為の積分値 
27	//---------------------------------------------------------------------------------------------------
28	// Variable definitions
29	
30	volatile float twoKp = twoKpDef;        // 2 * proportional gain (Kp)
31	volatile float twoKi = twoKiDef;        // 2 * integral gain (Ki)
32	volatile float q0 = 1.0f, q1 = 0.0f, q2 = 0.0f, q3 = 0.0f;  // quaternion of sensor frame relative to auxiliary frame
33	volatile float integralFBx = 0.0f,  integralFBy = 0.0f, integralFBz = 0.0f;    // integral error terms scaled by Ki
34	
35	//---------------------------------------------------------------------------------------------------
36	// AHRS algorithm update
37
この関数ですよ。ジャイロ、加速度、磁気から姿勢のクォータニオン（四元数）を求める
パラメータ：ジャイロ（オイラー角）、加速度（ベクトル）、磁気（ベクトル） 
38	void MahonyAHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz) {
39	    float recipNorm;
40	    float q0q0, q0q1, q0q2, q0q3, q1q1, q1q2, q1q3, q2q2, q2q3, q3q3;  
41	    float hx, hy, bx, bz;
42	    float halfvx, halfvy, halfvz, halfwx, halfwy, halfwz;
43	    float halfex, halfey, halfez;
44	    float qa, qb, qc;
45
センサーから加速度ベクトルの成分があれば、PI制御処理を続ける 
46	    // Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation)
47	    if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) {
48
加速度ベクトルを単位化する 
49	        // Normalise accelerometer measurement
50	        recipNorm = sqrt(ax * ax + ay * ay + az * az);
51	        ax /= recipNorm;
52	        ay /= recipNorm;
53	        az /= recipNorm;     
54
磁気ベクトルを単位化する 
55	        // Normalise magnetometer measurement
56	        recipNorm = sqrt(mx * mx + my * my + mz * mz);
57	        mx /= recipNorm;
58	        my /= recipNorm;
59	        mz /= recipNorm;   
60
重複の掛け算を避けるために、中間変数を用意する 
61	        // Auxiliary variables to avoid repeated arithmetic
62	        q0q0 = q0 * q0;
63	        q0q1 = q0 * q1;
64	        q0q2 = q0 * q2;
65	        q0q3 = q0 * q3;
66	        q1q1 = q1 * q1;
67	        q1q2 = q1 * q2;
68	        q1q3 = q1 * q3;
69	        q2q2 = q2 * q2;
70	        q2q3 = q2 * q3;
71	        q3q3 = q3 * q3;   
72
座標系は北をX、西をY、上空をZ軸とする。
ボディー座標系は装置の座標系で、参照座標系は地面座標である、一番最初に、ボディー座標系が参照座標系と重なっている！！
磁気センサーから取得した磁気ベクトルを姿勢のクォータニオン回転行列でボディー座標系から参照座標系へ変換する。
磁気ベクトルのY成分をX成分に移す。｛bx, by=0, bz}のようにする。なぜなら、参照座標系のY軸に磁力線が通らないからだ。 (クォータニオン回転行列について) 
73	        // Reference direction of Earth's magnetic field
74	        hx = 2.0f * (mx * (0.5f - q2q2 - q3q3) + my * (q1q2 - q0q3) + mz * (q1q3 + q0q2));
75	        hy = 2.0f * (mx * (q1q2 + q0q3) + my * (0.5f - q1q1 - q3q3) + mz * (q2q3 - q0q1));
76	        bx = sqrt(hx * hx + hy * hy);
77	        bz = 2.0f * (mx * (q1q3 - q0q2) + my * (q2q3 + q0q1) + mz * (0.5f - q1q1 - q2q2));
78
参照座標系の重力による加速度ベクトルは{ax=0,xy=0,az=1}（単位化）をクォータニオン回転行列の置換行列でボディー座標系に変換する。 (クォータニオン回転行列の置換行列について) 
79	        // Estimated direction of gravity and magnetic field
80	        halfvx = q1q3 - q0q2;
81	        halfvy = q0q1 + q2q3;
82	        halfvz = q0q0 - 0.5f + q3q3;
上記の参照座標系の磁気ベクトルをクォータニオン回転行列の置換行列でボディー座標系に変換し戻す。 
83	        halfwx = bx * (0.5f - q2q2 - q3q3) + bz * (q1q3 - q0q2);
84	        halfwy = bx * (q1q2 - q0q3) + bz * (q0q1 + q2q3);
85	        halfwz = bx * (q0q2 + q1q3) + bz * (0.5f - q1q1 - q2q2);  
86
センサーから取得したボディー座標系の加速度と{0,0,1}から作成したボディー座標系の加速度ベクトルを行列クロス乗算する。
２つの磁気ベクトルも行列クロス乗算する。
行列クロス乗算の結果を自動制御理論上の誤差として保持する。 
87	        // Error is sum of cross product between estimated direction and measured direction of field vectors
88	        halfex = (ay * halfvz - az * halfvy) + (my * halfwz - mz * halfwy);
89	        halfey = (az * halfvx - ax * halfvz) + (mz * halfwx - mx * halfwz);
90	        halfez = (ax * halfvy - ay * halfvx) + (mx * halfwy - my * halfwx);
91
誤差を積分制御係数と掛け算した結果を積分累計値に加える。
積分累計値をジャイロのベクトルの各成分に足す。 
92	        // Compute and apply integral feedback if enabled
93	        integralFBx += twoKi * halfex * (1.0f / sampleFreq);    // integral error scaled by Ki
94	        integralFBy += twoKi * halfey * (1.0f / sampleFreq);
95	        integralFBz += twoKi * halfez * (1.0f / sampleFreq);
96	        gx += integralFBx;    // apply integral feedback
97	        gy += integralFBy;
98	        gz += integralFBz;
99
誤差を比例制御係数と掛け算して、ジャイロベクトルの成分に足す 
100	        // Apply proportional feedback
101	        gx += twoKp * halfex;
102	        gy += twoKp * halfey;
103	        gz += twoKp * halfez;
104	    }
105
オイラー角であるジャイロの角速度と１周期の時間で姿勢のクォータニオンを変更させる。(オイラー角変化（微分）によりクォータニオンの変化を参照してくだい。) 
106	    // Integrate rate of change of quaternion
107	    gx *= (0.5f * (1.0f / sampleFreq));        // pre-multiply common factors
108	    gy *= (0.5f * (1.0f / sampleFreq));
109	    gz *= (0.5f * (1.0f / sampleFreq));
110	    qa = q0;
111	    qb = q1;
112	    qc = q2;
113	    q0 += (-qb * gx - qc * gy - q3 * gz);
114	    q1 += (qa * gx + qc * gz - q3 * gy);
115	    q2 += (qa * gy - qb * gz + q3 * gx);
116	    q3 += (qa * gz + qb * gy - qc * gx); 
117
姿勢のクォータニオンを単位化する 
118	    // Normalise quaternion
119	    recipNorm = sqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
120	    q0 /= recipNorm;
121	    q1 /= recipNorm;
122	    q2 /= recipNorm;
123	    q3 /= recipNorm;
124	}
=================================================
加速度、磁気から姿勢のクォータニオン（四元数）を初期化する関数 
125	void AHRSinit(double ax, double ay, double az, double mx, double my, double mz)// Inialize quaternion
126	{
127	    double initialRoll, initialPitch;
128	    double cosRoll, sinRoll, cosPitch, sinPitch;
129	    double magX, magY;
130	    double initialHdg, cosHeading, sinHeading;
センサーから取得した加速度からrollとpitchを算出する。重力がマイナス方向なので、atan2(-ay, -az)の-ayと-azがマイナスである。 一方、atan2(ax, -az)のaxにマイナスが付いていない理由は、pitchの回転の正方向に合わせるからである。 
132	    initialRoll = atan2(-ay, -az);
133	    initialPitch = atan2(ax, -az);
134	
135	    cosRoll = cos(initialRoll);
136	    sinRoll = sin(initialRoll);
137	    cosPitch = cos(initialPitch);
138	    sinPitch = sin(initialPitch);
139
センサーから取得した磁気の(mx,my,mz)をrollで回転してpitchで再回転して、参照座標系の磁気magYとmagXを算出します。算出方法の詳細については、「加速度センサー、磁気センサーから姿勢算出」を参照してください。 
140	    magX = mx * cosPitch + my * sinRoll * sinPitch + mz * cosRoll * sinPitch;
141	    magY = my * cosRoll - mz * sinRoll;
Heading(=yaw)の角度を算出する。-magYの-符号は、yawの回転の正方向に合わせるからである為に付けたのである。 
143	    initialHdg = atan2(-magY, magX);
144	
145	    cosRoll = cos(initialRoll * 0.5);
146	    sinRoll = sin(initialRoll * 0.5);
147	    cosPitch = cos(initialPitch * 0.5);
148	    sinPitch = sin(initialPitch * 0.5);
149	    cosHeading = cos(initialHdg * 0.5);
150	    sinHeading = sin(initialHdg * 0.5);
以下は単純にオイラー角からクォータニオン（四元数）へ変換するだけ。 
152	    q0 = cosRoll * cosPitch * cosHeading + sinRoll * sinPitch * sinHeading;
153	    q1 = sinRoll * cosPitch * cosHeading - cosRoll * sinPitch * sinHeading;
154	    q2 = cosRoll * sinPitch * cosHeading + sinRoll * cosPitch * sinHeading;
155	    q3 = cosRoll * cosPitch * sinHeading - sinRoll * sinPitch * cosHeading;
156	}
=================================================
ヘッダファイル 
	//=====================================================================================================
1	
2	// MahonyAHRS.h
3	//=====================================================================================================
4	//
5	// Madgwick's implementation of Mayhony's AHRS algorithm.
6	// See: http://www.x-io.co.uk/node/8#open_source_ahrs_and_imu_algorithms
7	//
8	// Date            Author            Notes
9	// 29/09/2011    SOH Madgwick    Initial release
10	// 02/10/2011    SOH Madgwick    Optimised for reduced CPU load
11	//
12	//=====================================================================================================
13	#ifndef MahonyAHRS_h
14	#define MahonyAHRS_h
15	
16	//----------------------------------------------------------------------------------------------------
17	// Variable declaration
18	
19	extern volatile float twoKp;            // 2 * proportional gain (Kp)
20	extern volatile float twoKi;            // 2 * integral gain (Ki)
21	extern volatile float q0, q1, q2, q3;    // quaternion of sensor frame relative to auxiliary frame
22	
23	//---------------------------------------------------------------------------------------------------
24	// Function declarations
25	
26	void MahonyAHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz);
27	void AHRSinit(double ax, double ay, double az, double mx, double my, double mz);
28	
29	#endif
30	//=====================================================================================================
31	// End of file
32	//=====================================================================================================