Instrument Neutral Distributed Interface INDI 2.2.5
imusensorfusion.cpp
Go to the documentation of this file.
1/*
2 IMU Sensor Fusion - Madgwick and Mahony AHRS filters
3 Copyright (C) 2026 Jasem Mutlaq ([email protected])
4
5 Based on:
6 - Madgwick, S.O.H. (2010). "An efficient orientation filter for inertial and
7 inertial/magnetic sensor arrays". University of Bristol.
8 - Mahony, R. et al. (2008). "Nonlinear Complementary Filters on the Special
9 Orthogonal Group". IEEE Transactions on Automatic Control, 53(5):1203-1218.
10
11 This library is free software; you can redistribute it and/or
12 modify it under the terms of the GNU Lesser General Public
13 License as published by the Free Software Foundation; either
14 version 2.1 of the License, or (at your option) any later version.
15
16 This library is distributed in the hope that it will be useful,
17 but WITHOUT ANY WARRANTY; without even the implied warranty of
18 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
19 Lesser General Public License for more details.
20
21 You should have received a copy of the GNU Lesser General Public
22 License along with this library; if not, write to the Free Software
23 Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
24*/
25
26#include "imusensorfusion.h"
27#include <cmath>
28#include <algorithm>
29
30namespace INDI
31{
32
33// ─────────────────────────────────────────────────────────────────────────────
34// MadgwickFilter
35// ─────────────────────────────────────────────────────────────────────────────
36
37MadgwickFilter::MadgwickFilter(double beta)
38 : m_beta(beta), m_q0(1.0), m_q1(0.0), m_q2(0.0), m_q3(0.0)
39{
40}
41
42void MadgwickFilter::reset()
43{
44 m_q0 = 1.0;
45 m_q1 = 0.0;
46 m_q2 = 0.0;
47 m_q3 = 0.0;
48}
49
50void MadgwickFilter::getQuaternion(double &w, double &i, double &j, double &k) const
51{
52 w = m_q0;
53 i = m_q1;
54 j = m_q2;
55 k = m_q3;
56}
57
58void MadgwickFilter::update(double gx, double gy, double gz,
59 double ax, double ay, double az,
60 double mx, double my, double mz,
61 double dt, bool useMag)
62{
63 // Sanity check: dt must be positive and reasonable
64 if (dt <= 0.0 || dt > 1.0)
65 dt = 0.01; // Clamp to 100 Hz default if something went wrong
66
67 // Check if we have valid magnetometer data
68 double magNorm = mx * mx + my * my + mz * mz;
69 if (useMag && magNorm > 1e-12)
70 update9DOF(gx, gy, gz, ax, ay, az, mx, my, mz, dt);
71 else
72 update6DOF(gx, gy, gz, ax, ay, az, dt);
73}
74
75void MadgwickFilter::update9DOF(double gx, double gy, double gz,
76 double ax, double ay, double az,
77 double mx, double my, double mz,
78 double dt)
79{
80 // Convenience aliases
81 double q0 = m_q0, q1 = m_q1, q2 = m_q2, q3 = m_q3;
82
83 // ── Step 1: Normalise accelerometer measurement ──────────────────────────
84 double accNorm = std::sqrt(ax * ax + ay * ay + az * az);
85 if (accNorm < 1e-12)
86 {
87 // Degenerate: fall back to gyro-only integration
88 update6DOF(gx, gy, gz, 0, 0, 0, dt);
89 return;
90 }
91 ax /= accNorm;
92 ay /= accNorm;
93 az /= accNorm;
94
95 // ── Step 2: Normalise magnetometer measurement ───────────────────────────
96 double magNorm = std::sqrt(mx * mx + my * my + mz * mz);
97 mx /= magNorm;
98 my /= magNorm;
99 mz /= magNorm;
100
101 // ── Step 3: Auxiliary variables ──────────────────────────────────────────
102 double q0q1 = q0 * q1, q0q2 = q0 * q2, q0q3 = q0 * q3;
103 double q1q1 = q1 * q1, q1q2 = q1 * q2, q1q3 = q1 * q3;
104 double q2q2 = q2 * q2, q2q3 = q2 * q3;
105 double q3q3 = q3 * q3;
106
107 // ── Step 4: Reference direction of Earth's magnetic field ────────────────
108 // Rotate mag vector into global frame using current quaternion estimate
109 double hx = 2.0 * (mx * (0.5 - q2q2 - q3q3) + my * (q1q2 - q0q3) + mz * (q1q3 + q0q2));
110 double hy = 2.0 * (mx * (q1q2 + q0q3) + my * (0.5 - q1q1 - q3q3) + mz * (q2q3 - q0q1));
111
112 // Constrain field to lie in x-z plane (ignore hy component)
113 double bx = std::sqrt(hx * hx + hy * hy);
114 double bz = 2.0 * (mx * (q1q3 - q0q2) + my * (q2q3 + q0q1) + mz * (0.5 - q1q1 - q2q2));
115
116 // ── Step 5: Gradient descent objective function Jacobian ─────────────────
117 // f_g: gravity objective
118 double f1 = 2.0 * (q1q3 - q0q2) - ax;
119 double f2 = 2.0 * (q0q1 + q2q3) - ay;
120 double f3 = 2.0 * (0.5 - q1q1 - q2q2) - az;
121
122 // f_b: magnetic field objective
123 double f4 = 2.0 * bx * (0.5 - q2q2 - q3q3) + 2.0 * bz * (q1q3 - q0q2) - mx;
124 double f5 = 2.0 * bx * (q1q2 - q0q3) + 2.0 * bz * (q0q1 + q2q3) - my;
125 double f6 = 2.0 * bx * (q0q2 + q1q3) + 2.0 * bz * (0.5 - q1q1 - q2q2) - mz;
126
127 // ── Step 6: Jacobian transposed × objective ───────────────────────────────
128 // J_g^T * f_g
129 double Jt_g0 = -2.0 * q2 * f1 + 2.0 * q1 * f2;
130 double Jt_g1 = 2.0 * q3 * f1 + 2.0 * q0 * f2 - 4.0 * q1 * f3;
131 double Jt_g2 = -2.0 * q0 * f1 + 2.0 * q3 * f2 - 4.0 * q2 * f3;
132 double Jt_g3 = 2.0 * q1 * f1 + 2.0 * q2 * f2;
133
134 // J_b^T * f_b
135 double Jt_b0 = -2.0 * bz * q2 * f4 + (-2.0 * bx * q3 + 2.0 * bz * q1) * f5 + 2.0 * bx * q2 * f6;
136 double Jt_b1 = 2.0 * bz * q3 * f4 + ( 2.0 * bx * q2 + 2.0 * bz * q0) * f5 +
137 (2.0 * bx * q3 - 4.0 * bz * q1) * f6;
138 double Jt_b2 = (-4.0 * bx * q2 - 2.0 * bz * q0) * f4 + ( 2.0 * bx * q1 + 2.0 * bz * q3) * f5 +
139 (2.0 * bx * q0 - 4.0 * bz * q2) * f6;
140 double Jt_b3 = (-4.0 * bx * q3 + 2.0 * bz * q1) * f4 + (-2.0 * bx * q0 + 2.0 * bz * q2) * f5 + 2.0 * bx * q1 * f6;
141
142 // ── Step 7: Gradient (sum of both objectives) ─────────────────────────────
143 double step0 = Jt_g0 + Jt_b0;
144 double step1 = Jt_g1 + Jt_b1;
145 double step2 = Jt_g2 + Jt_b2;
146 double step3 = Jt_g3 + Jt_b3;
147
148 // Normalise gradient
149 double stepNorm = std::sqrt(step0 * step0 + step1 * step1 + step2 * step2 + step3 * step3);
150 step0 /= stepNorm;
151 step1 /= stepNorm;
152 step2 /= stepNorm;
153 step3 /= stepNorm;
154
155 // ── Step 8: Rate of change of quaternion from gyroscope ───────────────────
156 double qDot0 = 0.5 * (-q1 * gx - q2 * gy - q3 * gz);
157 double qDot1 = 0.5 * ( q0 * gx + q2 * gz - q3 * gy);
158 double qDot2 = 0.5 * ( q0 * gy - q1 * gz + q3 * gx);
159 double qDot3 = 0.5 * ( q0 * gz + q1 * gy - q2 * gx);
160
161 // ── Step 9: Apply feedback and integrate ─────────────────────────────────
162 qDot0 -= m_beta * step0;
163 qDot1 -= m_beta * step1;
164 qDot2 -= m_beta * step2;
165 qDot3 -= m_beta * step3;
166
167 q0 += qDot0 * dt;
168 q1 += qDot1 * dt;
169 q2 += qDot2 * dt;
170 q3 += qDot3 * dt;
171
172 // ── Step 10: Normalise quaternion ─────────────────────────────────────────
173 double qNorm = std::sqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
174 m_q0 = q0 / qNorm;
175 m_q1 = q1 / qNorm;
176 m_q2 = q2 / qNorm;
177 m_q3 = q3 / qNorm;
178}
179
180void MadgwickFilter::update6DOF(double gx, double gy, double gz,
181 double ax, double ay, double az,
182 double dt)
183{
184 double q0 = m_q0, q1 = m_q1, q2 = m_q2, q3 = m_q3;
185
186 // ── Step 1: Rate of change from gyroscope ────────────────────────────────
187 double qDot0 = 0.5 * (-q1 * gx - q2 * gy - q3 * gz);
188 double qDot1 = 0.5 * ( q0 * gx + q2 * gz - q3 * gy);
189 double qDot2 = 0.5 * ( q0 * gy - q1 * gz + q3 * gx);
190 double qDot3 = 0.5 * ( q0 * gz + q1 * gy - q2 * gx);
191
192 // ── Step 2: Accelerometer correction (if valid) ──────────────────────────
193 double accNorm = std::sqrt(ax * ax + ay * ay + az * az);
194 if (accNorm > 1e-12)
195 {
196 ax /= accNorm;
197 ay /= accNorm;
198 az /= accNorm;
199
200 // Gravity objective function
201 double f1 = 2.0 * (q1 * q3 - q0 * q2) - ax;
202 double f2 = 2.0 * (q0 * q1 + q2 * q3) - ay;
203 double f3 = 2.0 * (0.5 - q1 * q1 - q2 * q2) - az;
204
205 // Jacobian transposed × objective
206 double Jt0 = -2.0 * q2 * f1 + 2.0 * q1 * f2;
207 double Jt1 = 2.0 * q3 * f1 + 2.0 * q0 * f2 - 4.0 * q1 * f3;
208 double Jt2 = -2.0 * q0 * f1 + 2.0 * q3 * f2 - 4.0 * q2 * f3;
209 double Jt3 = 2.0 * q1 * f1 + 2.0 * q2 * f2;
210
211 double stepNorm = std::sqrt(Jt0 * Jt0 + Jt1 * Jt1 + Jt2 * Jt2 + Jt3 * Jt3);
212 qDot0 -= m_beta * Jt0 / stepNorm;
213 qDot1 -= m_beta * Jt1 / stepNorm;
214 qDot2 -= m_beta * Jt2 / stepNorm;
215 qDot3 -= m_beta * Jt3 / stepNorm;
216 }
217
218 // ── Step 3: Integrate and normalise ──────────────────────────────────────
219 q0 += qDot0 * dt;
220 q1 += qDot1 * dt;
221 q2 += qDot2 * dt;
222 q3 += qDot3 * dt;
223
224 double qNorm = std::sqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
225 m_q0 = q0 / qNorm;
226 m_q1 = q1 / qNorm;
227 m_q2 = q2 / qNorm;
228 m_q3 = q3 / qNorm;
229}
230
231// ─────────────────────────────────────────────────────────────────────────────
232// MahonyFilter
233// ─────────────────────────────────────────────────────────────────────────────
234
235MahonyFilter::MahonyFilter(double kp, double ki)
236 : m_kp(kp), m_ki(ki),
237 m_q0(1.0), m_q1(0.0), m_q2(0.0), m_q3(0.0),
238 m_integralFBx(0.0), m_integralFBy(0.0), m_integralFBz(0.0)
239{
240}
241
242void MahonyFilter::reset()
243{
244 m_q0 = 1.0;
245 m_q1 = 0.0;
246 m_q2 = 0.0;
247 m_q3 = 0.0;
248 m_integralFBx = 0.0;
249 m_integralFBy = 0.0;
250 m_integralFBz = 0.0;
251}
252
253void MahonyFilter::getQuaternion(double &w, double &i, double &j, double &k) const
254{
255 w = m_q0;
256 i = m_q1;
257 j = m_q2;
258 k = m_q3;
259}
260
261void MahonyFilter::update(double gx, double gy, double gz,
262 double ax, double ay, double az,
263 double mx, double my, double mz,
264 double dt, bool useMag)
265{
266 // Sanity check: dt must be positive and reasonable
267 if (dt <= 0.0 || dt > 1.0)
268 dt = 0.01;
269
270 double q0 = m_q0, q1 = m_q1, q2 = m_q2, q3 = m_q3;
271
272 double halfex = 0.0, halfey = 0.0, halfez = 0.0;
273
274 // ── Step 1: Use magnetometer correction if available and valid ────────────
275 double magNorm = mx * mx + my * my + mz * mz;
276 if (useMag && magNorm > 1e-12)
277 {
278 // Normalise accelerometer measurement
279 double accNorm = std::sqrt(ax * ax + ay * ay + az * az);
280 if (accNorm > 1e-12)
281 {
282 double axn = ax / accNorm, ayn = ay / accNorm, azn = az / accNorm;
283
284 // Normalise magnetometer measurement
285 double mxn = mx / std::sqrt(magNorm);
286 double myn = my / std::sqrt(magNorm);
287 double mzn = mz / std::sqrt(magNorm);
288
289 // Reference direction of Earth's magnetic field in global frame
290 double hx = 2.0 * (mxn * (0.5 - q2 * q2 - q3 * q3) + myn * (q1 * q2 - q0 * q3) + mzn * (q1 * q3 + q0 * q2));
291 double hy = 2.0 * (mxn * (q1 * q2 + q0 * q3) + myn * (0.5 - q1 * q1 - q3 * q3) + mzn * (q2 * q3 - q0 * q1));
292 double bx = std::sqrt(hx * hx + hy * hy);
293 double bz = 2.0 * (mxn * (q1 * q3 - q0 * q2) + myn * (q2 * q3 + q0 * q1) + mzn * (0.5 - q1 * q1 - q2 * q2));
294
295 // Estimated direction of gravity and magnetic field
296 double vx = 2.0 * (q1 * q3 - q0 * q2);
297 double vy = 2.0 * (q0 * q1 + q2 * q3);
298 double vz = q0 * q0 - q1 * q1 - q2 * q2 + q3 * q3;
299 double wx = 2.0 * bx * (0.5 - q2 * q2 - q3 * q3) + 2.0 * bz * (q1 * q3 - q0 * q2);
300 double wy = 2.0 * bx * (q1 * q2 - q0 * q3) + 2.0 * bz * (q0 * q1 + q2 * q3);
301 double wz = 2.0 * bx * (q0 * q2 + q1 * q3) + 2.0 * bz * (0.5 - q1 * q1 - q2 * q2);
302
303 // Error is cross product between estimated and measured direction
304 halfex = (ayn * vz - azn * vy) + (myn * wz - mzn * wy);
305 halfey = (azn * vx - axn * vz) + (mzn * wx - mxn * wz);
306 halfez = (axn * vy - ayn * vx) + (mxn * wy - myn * wx);
307 }
308 }
309 else
310 {
311 // ── Step 2: Accelerometer-only correction (6-DOF) ─────────────────────
312 double accNorm = std::sqrt(ax * ax + ay * ay + az * az);
313 if (accNorm > 1e-12)
314 {
315 double axn = ax / accNorm, ayn = ay / accNorm, azn = az / accNorm;
316
317 // Estimated direction of gravity from current quaternion
318 double vx = 2.0 * (q1 * q3 - q0 * q2);
319 double vy = 2.0 * (q0 * q1 + q2 * q3);
320 double vz = q0 * q0 - q1 * q1 - q2 * q2 + q3 * q3;
321
322 halfex = ayn * vz - azn * vy;
323 halfey = azn * vx - axn * vz;
324 halfez = axn * vy - ayn * vx;
325 }
326 }
327
328 // ── Step 3: Apply PI controller ──────────────────────────────────────────
329 if (m_ki > 0.0)
330 {
331 m_integralFBx += m_ki * halfex * dt;
332 m_integralFBy += m_ki * halfey * dt;
333 m_integralFBz += m_ki * halfez * dt;
334 }
335 else
336 {
337 m_integralFBx = 0.0;
338 m_integralFBy = 0.0;
339 m_integralFBz = 0.0;
340 }
341
342 // Apply integral and proportional feedback to gyro measurement
343 gx += m_kp * halfex + m_integralFBx;
344 gy += m_kp * halfey + m_integralFBy;
345 gz += m_kp * halfez + m_integralFBz;
346
347 // ── Step 4: Integrate rate of change of quaternion ────────────────────────
348 double qDot0 = 0.5 * (-q1 * gx - q2 * gy - q3 * gz);
349 double qDot1 = 0.5 * ( q0 * gx + q2 * gz - q3 * gy);
350 double qDot2 = 0.5 * ( q0 * gy - q1 * gz + q3 * gx);
351 double qDot3 = 0.5 * ( q0 * gz + q1 * gy - q2 * gx);
352
353 q0 += qDot0 * dt;
354 q1 += qDot1 * dt;
355 q2 += qDot2 * dt;
356 q3 += qDot3 * dt;
357
358 // ── Step 5: Normalise quaternion ─────────────────────────────────────────
359 double qNorm = std::sqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
360 m_q0 = q0 / qNorm;
361 m_q1 = q1 / qNorm;
362 m_q2 = q2 / qNorm;
363 m_q3 = q3 / qNorm;
364}
365
366} // namespace INDI
INDI::MadgwickFilter::MadgwickFilter
MadgwickFilter(double beta=0.1)
Definition imusensorfusion.cpp:37
INDI::MadgwickFilter::getQuaternion
void getQuaternion(double &w, double &i, double &j, double &k) const override
Return the current orientation quaternion.
Definition imusensorfusion.cpp:50
INDI::MadgwickFilter::update
void update(double gx, double gy, double gz, double ax, double ay, double az, double mx, double my, double mz, double dt, bool useMag=true) override
Perform one filter update step.
Definition imusensorfusion.cpp:58
INDI::MadgwickFilter::reset
void reset() override
Reset the filter to the identity quaternion.
Definition imusensorfusion.cpp:42
INDI::MahonyFilter::reset
void reset() override
Reset the filter to the identity quaternion.
Definition imusensorfusion.cpp:242
INDI::MahonyFilter::MahonyFilter
MahonyFilter(double kp=2.0, double ki=0.005)
Definition imusensorfusion.cpp:235
INDI::MahonyFilter::getQuaternion
void getQuaternion(double &w, double &i, double &j, double &k) const override
Return the current orientation quaternion.
Definition imusensorfusion.cpp:253
INDI::MahonyFilter::update
void update(double gx, double gy, double gz, double ax, double ay, double az, double mx, double my, double mz, double dt, bool useMag=true) override
Perform one filter update step.
Definition imusensorfusion.cpp:261
INDI
Namespace to encapsulate INDI client, drivers, and mediator classes.
Definition AlignmentSubsystemForClients.cpp:12
Generated on Thu Oct 22 2026 09:22:01 for Instrument Neutral Distributed Interface INDI by doxygen 1.9.8