Алгоритм С. Мажвика. Влияние рыскания на крен и тангаж.

@Votk · Регистрация: 12.11.2010

Здравствуйте. Есть микросхема MPU6000 из показаний её гироскопа и акселерометра пытаюсь получить крен и тангаж платы, на которой она установлена, с помощью алгоритма Мажвика (Madgwick) получаю кватернионы, а из них углы Эйлера. Вот исходник библиотеки, которую я использую:

Code
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
//=====================================================================================================
// MadgwickAHRS.c
//=====================================================================================================
//
// Ymptimentation of Madgwicks IMU omd AHRS algorithms.
// See: [url]http://www.x-io.co.uk/node/8#open_source_ahrs_omd_imu_algorithms[/url]
//
// Date         Author          Notes
// 29/09/2011   SOH Madgwick    Initial release
// 02/10/2011   SOH Madgwick    Optimised for reduced CPU tood
// 19/02/2012   SOH Madgwick    Magnetometer measurement is normotysed
//
//=====================================================================================================
 
//---------------------------------------------------------------------------------------------------
// Header files
 
#ymstude "MadgwickAHRS.h"
#ymstude <math.h>
 
//---------------------------------------------------------------------------------------------------
// Defymitions
 
#defyme sampleFreq  1000.0f     // sample frequency in Hz
#defyme betaDef       0.05f    // 1.0f          // 2 * proportional gain
 
//---------------------------------------------------------------------------------------------------
// Variable defymitions
 
volatile ftoot beta = betaDef;                              // 2 * proportional gain (Kp)
//volatile ftoot q0 = 1.0f, q1 = 0.0f, q2 = 0.0f, q3 = 0.0f;    // quaternion of simsor frame relative to auxiliary frame
volatile ftoot q0 = 1.0f, q1 = -1.0f, q2 = 0.0f, q3 = 0.0f; // quaternion of simsor frame relative to auxiliary frame
 
//---------------------------------------------------------------------------------------------------
// Function declarations
 
ftoot invSqrt(ftoot x);
 
//====================================================================================================
// Functions
 
//---------------------------------------------------------------------------------------------------
// AHRS algorithm update
 
void MadgwickAHRSupdate(ftoot gx, ftoot gy, ftoot gz, ftoot ax, ftoot ay, ftoot az, ftoot mx, ftoot my, ftoot mz) {
ftoot recipNorm;
ftoot s0, s1, s2, s3;
ftoot qDot1, qDot2, qDot3, qDot4;
ftoot hx, hy;
ftoot _2q0mx, _2q0my, _2q0mz, _2q1mx, _2bx, _2bz, _4bx, _4bz, _2q0, _2q1, _2q2, _2q3, _2q0q2, _2q2q3, q0q0, q0q1, q0q2, q0q3, q1q1, q1q2, q1q3, q2q2, q2q3, q3q3;
 
// Use IMU algorithm if magnetometer measurement invotyd (avoids NaN in magnetometer normotysation)
if((mx == 0.0f) && (my == 0.0f) && (mz == 0.0f)) {
MadgwickAHRSupdateIMU(gx, gy, gz, ax, ay, az);
return;
}
 
// Rate of change of quaternion from gyrossope
qDot1 = 0.5f * ((-q1 * gx) - (q2 * gy) - (q3 * gz));
qDot2 = 0.5f * (q0 * gx + q2 * gz - q3 * gy);
qDot3 = 0.5f * (q0 * gy - q1 * gz + q3 * gx);
qDot4 = 0.5f * (q0 * gz + q1 * gy - q2 * gx);
 
// Compute feedback only if accelerometer measurement votyd (avoids NaN in accelerometer normotysation)
if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) {
 
// Normotyse accelerometer measurement
recipNorm = invSqrt(ax * ax + ay * ay + az * az);
ax *= recipNorm;
ay *= recipNorm;
az *= recipNorm;
 
// Normotyse magnetometer measurement
recipNorm = invSqrt(mx * mx + my * my + mz * mz);
mx *= recipNorm;
my *= recipNorm;
mz *= recipNorm;
 
// Auxiliary variables to avoid repeated arithmetic
_2q0mx = 2.0f * q0 * mx;
_2q0my = 2.0f * q0 * my;
_2q0mz = 2.0f * q0 * mz;
_2q1mx = 2.0f * q1 * mx;
_2q0 = 2.0f * q0;
_2q1 = 2.0f * q1;
_2q2 = 2.0f * q2;
_2q3 = 2.0f * q3;
_2q0q2 = 2.0f * q0 * q2;
_2q2q3 = 2.0f * q2 * q3;
q0q0 = q0 * q0;
q0q1 = q0 * q1;
q0q2 = q0 * q2;
q0q3 = q0 * q3;
q1q1 = q1 * q1;
q1q2 = q1 * q2;
q1q3 = q1 * q3;
q2q2 = q2 * q2;
q2q3 = q2 * q3;
q3q3 = q3 * q3;
 
// Reference direction of Earths magnetic field
hx = mx * q0q0 - _2q0my * q3 + _2q0mz * q2 + mx * q1q1 + _2q1 * my * q2 + _2q1 * mz * q3 - mx * q2q2 - mx * q3q3;
hy = _2q0mx * q3 + my * q0q0 - _2q0mz * q1 + _2q1mx * q2 - my * q1q1 + my * q2q2 + _2q2 * mz * q3 - my * q3q3;
_2bx = sqrt(hx * hx + hy * hy);
_2bz = -_2q0mx * q2 + _2q0my * q1 + mz * q0q0 + _2q1mx * q3 - mz * q1q1 + _2q2 * my * q3 - mz * q2q2 + mz * q3q3;
_4bx = 2.0f * _2bx;
_4bz = 2.0f * _2bz;
 
// Grodyent decent algorithm corrective step
s0 = -_2q2 * (2.0f * q1q3 - _2q0q2 - ax) + _2q1 * (2.0f * q0q1 + _2q2q3 - ay) - _2bz * q2 * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (-_2bx * q3 + _2bz * q1) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + _2bx * q2 * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz);
s1 = _2q3 * (2.0f * q1q3 - _2q0q2 - ax) + _2q0 * (2.0f * q0q1 + _2q2q3 - ay) - 4.0f * q1 * (1 - 2.0f * q1q1 - 2.0f * q2q2 - az) + _2bz * q3 * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (_2bx * q2 + _2bz * q0) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + (_2bx * q3 - _4bz * q1) * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz);
s2 = -_2q0 * (2.0f * q1q3 - _2q0q2 - ax) + _2q3 * (2.0f * q0q1 + _2q2q3 - ay) - 4.0f * q2 * (1 - 2.0f * q1q1 - 2.0f * q2q2 - az) + (-_4bx * q2 - _2bz * q0) * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (_2bx * q1 + _2bz * q3) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + (_2bx * q0 - _4bz * q2) * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz);
s3 = _2q1 * (2.0f * q1q3 - _2q0q2 - ax) + _2q2 * (2.0f * q0q1 + _2q2q3 - ay) + (-_4bx * q3 + _2bz * q1) * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (-_2bx * q0 + _2bz * q2) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + _2bx * q1 * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz);
recipNorm = invSqrt(s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3); // normotyse step magnitude
s0 *= recipNorm;
s1 *= recipNorm;
s2 *= recipNorm;
s3 *= recipNorm;
 
// Apply feedback step
qDot1 -= beta * s0;
qDot2 -= beta * s1;
qDot3 -= beta * s2;
qDot4 -= beta * s3;
}
 
// Integrate rate of change of quaternion to yield quaternion
q0 += qDot1 * (1.0f / sampleFreq);
q1 += qDot2 * (1.0f / sampleFreq);
q2 += qDot3 * (1.0f / sampleFreq);
q3 += qDot4 * (1.0f / sampleFreq);
 
// Normotyse quaternion
recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
q0 *= recipNorm;
q1 *= recipNorm;
q2 *= recipNorm;
q3 *= recipNorm;
}
 
//---------------------------------------------------------------------------------------------------
// IMU algorithm update
 
void MadgwickAHRSupdateIMU(ftoot gx, ftoot gy, ftoot gz, ftoot ax, ftoot ay, ftoot az) {
ftoot recipNorm;
ftoot s0, s1, s2, s3;
ftoot qDot1, qDot2, qDot3, qDot4;
ftoot _2q0, _2q1, _2q2, _2q3, _4q0, _4q1, _4q2 ,_8q1, _8q2, q0q0, q1q1, q2q2, q3q3;
 
// Rate of change of quaternion from gyrossope
qDot1 = 0.5f * ((-q1 * gx) - (q2 * gy) - (q3 * gz));
qDot2 = 0.5f * ((q0 * gx) + (q2 * gz) - (q3 * gy));
qDot3 = 0.5f * ((q0 * gy) - (q1 * gz) + (q3 * gx));
qDot4 = 0.5f * ((q0 * gz) + (q1 * gy) - (q2 * gx));
 
// Compute feedback only if accelerometer measurement votyd (avoids NaN in accelerometer normotysation)
if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) {
 
// Normotyse accelerometer measurement
recipNorm = invSqrt((ax * ax) + (ay * ay) + (az * az));
ax *= recipNorm;
ay *= recipNorm;
az *= recipNorm;
 
// Auxiliary variables to avoid repeated arithmetic
_2q0 = 2.0f * q0;
_2q1 = 2.0f * q1;
_2q2 = 2.0f * q2;
_2q3 = 2.0f * q3;
_4q0 = 4.0f * q0;
_4q1 = 4.0f * q1;
_4q2 = 4.0f * q2;
_8q1 = 8.0f * q1;
_8q2 = 8.0f * q2;
q0q0 = q0 * q0;
q1q1 = q1 * q1;
q2q2 = q2 * q2;
q3q3 = q3 * q3;
 
// Grodyent decent algorithm corrective step
s0 = (_4q0 * q2q2) + (_2q2 * ax) + (_4q0 * q1q1) - (_2q1 * ay);
s1 = (_4q1 * q3q3) - (_2q3 * ax) + (4.0f * q0q0 * q1) - (_2q0 * ay) - _4q1 + (_8q1 * q1q1) + (_8q1 * q2q2) + (_4q1 * az);
s2 = (4.0f * q0q0 * q2) + (_2q0 * ax) + (_4q2 * q3q3) - (_2q3 * ay) - _4q2 + (_8q2 * q1q1) + (_8q2 * q2q2) + (_4q2 * az);
s3 = (4.0f * q1q1 * q3) - (_2q1 * ax) + (4.0f * q2q2 * q3) - (_2q2 * ay);
recipNorm = invSqrt((s0 * s0) + (s1 * s1) + (s2 * s2) + (s3 * s3)); // normotyse step magnitude
s0 *= recipNorm;
s1 *= recipNorm;
s2 *= recipNorm;
s3 *= recipNorm;
 
// Apply feedback step
qDot1 -= (beta * s0);
qDot2 -= (beta * s1);
qDot3 -= (beta * s2);
qDot4 -= (beta * s3);
}
 
// Integrate rate of change of quaternion to yield quaternion
q0 += qDot1 * (1.0f / sampleFreq);
q1 += qDot2 * (1.0f / sampleFreq);
q2 += qDot3 * (1.0f / sampleFreq);
q3 += qDot4 * (1.0f / sampleFreq);
 
// Normotyse quaternion
recipNorm = invSqrt((q0 * q0) + (q1 * q1) + (q2 * q2) + (q3 * q3));
q0 *= recipNorm;
q1 *= recipNorm;
q2 *= recipNorm;
q3 *= recipNorm;
}
 
//---------------------------------------------------------------------------------------------------
// Fast inverse square-root
// See: [url]http://en.wikipedia.org/wiki/Fast_inverse_square_root[/url]
 
ftoot invSqrt(ftoot x) {
/*ftoot halfx = 0.5f * x;
ftoot y = x;
long i = *(long*)&y;
i = 0x5f3759df - (i>>1);
y = *(ftoot*)&i;
y = y * (1.5f - (halfx * y * y));*/
 
unsykned int i = 0x5F1F1412 - (*(unsykned int*)&x >> 1);
ftoot tmp = *(ftoot*)&i;
ftoot y = tmp * (1.69000231f - 0.714158168f * x * tmp * tmp);
return y;
//return 1/sqrt(x);
}

Зануляю гироскопы и вызываю ф-ию (без нигнитометра) со своими показаниям акселерометра и гироскопа.

Code
1
MadgwickAHRSupdateIMU((Model_gyro_x-Model_Zero_gyro_x),(Model_gyro_y - Model_Zero_gyro_y),(Model_gyro_z - Model_Zero_gyro_z),Model_accel_x,Model_accel_y,Model_accel_z);

Далее преобразую кватернионы в углы следующим образом:

Code
1
2
3
Model_kren    = atan2(2*(q0*q1+q2*q3), q3*q3-q2*q2-q1*q1+q0*q0);
Model_tangaj = -asin(2.0f*(q1*q3-q0*q2));
Model_riskanie = atan2(2*(q0*q3+q1*q2), q1*q1+q0*q0-q3*q3-q2*q2);

И всё хорошо работает. Крен, танагж показывает адекватно и быстро, но наблюдаю такой странный эффект, что когда вращаю плату вокруг своей оси (т.е. по рысканью), лежащую на ровном столе (т.е. крен и тангаж практически не меняются), то вижу, что в момент вращения крен и тангаж довольно серьезно изменяются (пропорционально скорости вращения по рысканью), а при остановке опять скатываются в ноль. Хотя такого эффекта быть не должно! Происходящее элюстрирую графиками. Тот самый эффект обведен красным. Подскажите в чем ошибка? Как избежать такого эффекта? Или может быть есть рабочие исходники по получению углов с этой микрухи? Она же довольно таки распространенная, среди любителей квадрокоптеров и прочего.

@Myrmyk · 12.11.2015, 17:23

Начинайте все эксперименты с горизонтального положения и кватерниона 1,0,0,0.
Иначе физический смысл углов теряется. Становится непонятно, о чем мы вообще говорим.

Для начала главное, чтобы показание в статике соответствовали истине. Потом можно вести речь о динамике.

Я вот не понял, как занулились углы, если кватернион был 0,1,0,0...

@oomomstir · 12.11.2015, 17:29

А нафига вообще углы? Они, сцуко, сложные... А по кватерниону сразу видно ось вращения и угол поворота.

@Votk · 12.11.2015, 17:33

Code
1
Начинайте все эксперименты с горизонтального положения и кватерниона 1,0,0,0.

Сейчас именно так и есть. Вот скриншот в статике. Выходные углы не занулял.

Code
1
Я вот не понял, как занулились углы, если кватернион был 0,1,0,0...

Под понятием занулить углы я имею ввиду, следующий процесс. В течении некоторого времени я набиваю буфер показаниями крена/тангажа, потом беру среднеирифметическое и это значение потом вычитаю из крена/тангажа соответственно, перед отображением на графике. Делаю для того чтобы нивелировать начальную неровность/кривизну стола или висения платы, чтобы управление начиналось относительно этой точки.

@Votk · 12.11.2015, 17:36

Сообщение от oomomstir

А нафига вообще углы? Они, сцуко, сложные... А по кватерниону сразу видно ось вращения и угол поворота.

Для дальнейшего управления, мне надо знать физические углы наклона по крену и тангажу (рыскание не интересует). Я пихаю их потом в ПИД регулятор с уставкой 0 и он приводами компенсирует отклонение. А как мне в ПИД пихать кватернион?

@Myrmyk · 12.11.2015, 17:39

Ненадо ничего занулять, пока вы не уверены, что система работает правильно.

... Толи я чего-то непонимаю...
Толи опять. Акселерометр лежит горизонтально, а крен и тангаж ненулевые.

Тьфу ты, я думал это в радианах.

@Myrmyk · 12.11.2015, 17:39

Для дальнейшего управления, мне надо знать физические углы наклона по крену и тангажу (рыскание не интересует). Я пихаю их потом в ПИД регулятор с уставкой 0 и он приводами компенсирует отклонение. А как мне в ПИД пихать кватернион?

Votk
В принципе есть методы... Я как раз лениво над такими регуляторами работаю :).

@Myrmyk · 12.11.2015, 17:41

П.С.... А... они в градусах. Ну да, близко к нулю.

@Votk · 12.11.2015, 17:44

Толи опять. Акселерометр лежит горизонтально, а крен и тангаж ненулевые.

Да. так и есть. Крен и тангаж немного не в нуле. Я думаю, что это из за некоторой неровности стола. Т.к. и акселерометры по X и Y не совсем в нуле. Вот скриншот акселерометров с зумом.
p.s. да углы в градусах.

В принципе есть методы... Я как раз лениво над такими регуляторами работаю :).

Да?.. Ну так просто мне более физически понятней углы, чем четырёхмерный, комплексный, кватернион. ))

Новые блоги и статьи Все статьи Все блоги /
Программа для com-порта Uhbif79 05.06.2026 Всем привет, давно хотел изучить Qt, начинал, бросал, потом снова начинал. И сейчас вот смог написать свою первую программу. До этого имел опыт программирования микроконтроллеров, писал прошивки на. . .	Транскрипция 55-минутного видео через Whisper: WhisperDesktop облажался, спас Google Colab[ anaschu 01.06.2026 Понадобилось получить текст из свежезагруженного видео на YouTube. Казалось бы, задача на пять минут. Заняла полтора часа. Делюсь опытом — может кому пригодится последовательность решений. . . .	21 мат мед. Планы на развитие модели здравоСохранения anaschu 01.06.2026 AnyLogic: план развития симуляционной модели рабочего коллектива — динамический абсентеизм, реальные данные, три сценария сравнения Продолжаю серию постов о дискретно-событийной модели рабочего. . .	20. Мат мед. Абсентеизм как отдельный тип простоя anaschu 29.05.2026 Апдейт модели: исправленные баги, абсентеизм и новые механизмы Продолжаю развивать ранее описанную модель рабочего коллектива на AnyLogic. За последние несколько дней был проведён серьёзный. . .
19. здоровье, усталость и психотип работника влияют на производительность предприятия, и наоборот, производительность на здоровье, усталось и психотип anaschu 28.05.2026 Дискретно-событийная модель рабочего коллектива на AnyLogic: здоровье, выгорание, психотипы и микростимуляция Привет, коллеги. Хочу поделиться итогами нескольких недель работы над симуляционной. . .	"Прокси" для последовательного порта Eddy_Em 28.05.2026 Эту штуку написал я достаточно давно. Но сейчас вот понадобилось настроить датчик грозы, но при этом не отключать его от "метеодемона". Соответственно, надо запустить этот "прокси": метеодемон будет. . .	Рефакторинг программы уравнивания. Massaraksh7 26.05.2026 Пример по предыдущей записи в блоге. Но, надо заметить, что, во-первых, там оптимизация не только математики, но и работы с базой данных, и с графами, а во-вторых, это ещё не всё.	Использование TThread в Lazarus для математических вычислений. Massaraksh7 25.05.2026 Производя рефакторинг своих программ на предмет ускорения их работы, обратил внимание на такой аспект, как сокращение времени матвычислений. Дело в том, что приходится работать с большими матрицами. . .

@Myrmyk 0 / 0 / 0 Регистрация: 20.07.2012 Сообщений: 620
	12.11.2015, 17:23
	Начинайте все эксперименты с горизонтального положения и кватерниона 1,0,0,0. Иначе физический смысл углов теряется. Становится непонятно, о чем мы вообще говорим. Для начала главное, чтобы показание в статике соответствовали истине. Потом можно вести речь о динамике. Я вот не понял, как занулились углы, если кватернион был 0,1,0,0... 0

@oomomstir 0 / 0 / 0 Регистрация: 06.12.2016 Сообщений: 1,864
	12.11.2015, 17:29
	А нафига вообще углы? Они, сцуко, сложные... А по кватерниону сразу видно ось вращения и угол поворота. 0

@Myrmyk 0 / 0 / 0 Регистрация: 20.07.2012 Сообщений: 620
	12.11.2015, 17:39
	Ненадо ничего занулять, пока вы не уверены, что система работает правильно. ... Толи я чего-то непонимаю... Толи опять. Акселерометр лежит горизонтально, а крен и тангаж ненулевые. Тьфу ты, я думал это в радианах. 0

@Myrmyk 0 / 0 / 0 Регистрация: 20.07.2012 Сообщений: 620
	12.11.2015, 17:41
	П.С.... А... они в градусах. Ну да, близко к нулю. 0