Directory: | ./ |
---|---|
File: | src/filters/madgwickahrs.cpp |
Date: | 2024-08-13 12:13:25 |
Exec | Total | Coverage | |
---|---|---|---|
Lines: | 97 | 111 | 87.4% |
Branches: | 63 | 162 | 38.9% |
Line | Branch | Exec | Source |
---|---|---|---|
1 | //========================================================================= | ||
2 | // | ||
3 | // Implementation of Madgwick's IMU and AHRS algorithms. | ||
4 | // See: http://www.x-io.co.uk/node/8#open_source_ahrs_and_imu_algorithms | ||
5 | // | ||
6 | // Date Author Notes | ||
7 | // 29/09/2011 SOH Madgwick Initial release | ||
8 | // 02/10/2011 SOH Madgwick Optimised for reduced CPU load | ||
9 | // 11/05/2017 T Flayols Make it a dynamic-graph entity | ||
10 | // | ||
11 | //========================================================================= | ||
12 | |||
13 | #include <dynamic-graph/all-commands.h> | ||
14 | #include <dynamic-graph/factory.h> | ||
15 | |||
16 | #include <sot/core/debug.hh> | ||
17 | #include <sot/core/madgwickahrs.hh> | ||
18 | #include <sot/core/stop-watch.hh> | ||
19 | |||
20 | namespace dynamicgraph { | ||
21 | namespace sot { | ||
22 | namespace dg = ::dynamicgraph; | ||
23 | using namespace dg; | ||
24 | using namespace dg::command; | ||
25 | using namespace std; | ||
26 | |||
27 | typedef Vector6d Vector6; | ||
28 | |||
29 | #define PROFILE_MADGWICKAHRS_COMPUTATION "MadgwickAHRS computation" | ||
30 | |||
31 | #define INPUT_SIGNALS m_accelerometerSIN << m_gyroscopeSIN | ||
32 | #define OUTPUT_SIGNALS m_imu_quatSOUT | ||
33 | |||
34 | /// Define EntityClassName here rather than in the header file | ||
35 | /// so that it can be used by the macros DEFINE_SIGNAL_**_FUNCTION. | ||
36 | typedef MadgwickAHRS EntityClassName; | ||
37 | |||
38 | /* --- DG FACTORY ---------------------------------------------------- */ | ||
39 | ✗ | DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(MadgwickAHRS, "MadgwickAHRS"); | |
40 | |||
41 | /* ------------------------------------------------------------------- */ | ||
42 | /* --- CONSTRUCTION -------------------------------------------------- */ | ||
43 | /* ------------------------------------------------------------------- */ | ||
44 | 1 | MadgwickAHRS::MadgwickAHRS(const std::string &name) | |
45 | : Entity(name), | ||
46 |
7/14✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 1 times.
✗ Branch 21 not taken.
|
1 | CONSTRUCT_SIGNAL_IN(accelerometer, dynamicgraph::Vector), |
47 |
7/14✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 1 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 1 times.
✗ Branch 13 not taken.
✓ Branch 15 taken 1 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 1 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 1 times.
✗ Branch 22 not taken.
|
1 | CONSTRUCT_SIGNAL_IN(gyroscope, dynamicgraph::Vector), |
48 |
10/20✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 1 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 1 times.
✗ Branch 13 not taken.
✓ Branch 15 taken 1 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 1 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 1 times.
✗ Branch 22 not taken.
✓ Branch 24 taken 1 times.
✗ Branch 25 not taken.
✓ Branch 27 taken 1 times.
✗ Branch 28 not taken.
✓ Branch 30 taken 1 times.
✗ Branch 31 not taken.
|
1 | CONSTRUCT_SIGNAL_OUT(imu_quat, dynamicgraph::Vector, |
49 | m_gyroscopeSIN << m_accelerometerSIN), | ||
50 | 1 | m_initSucceeded(false), | |
51 | 1 | m_beta(betaDef), | |
52 | 1 | m_q0(1.0), | |
53 | 1 | m_q1(0.0), | |
54 | 1 | m_q2(0.0), | |
55 | 1 | m_q3(0.0), | |
56 | 1 | m_sampleFreq(512.0) { | |
57 |
3/6✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
|
1 | Entity::signalRegistration(INPUT_SIGNALS << OUTPUT_SIGNALS); |
58 | |||
59 | /* Commands. */ | ||
60 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
1 | addCommand("init", |
61 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | makeCommandVoid1(*this, &MadgwickAHRS::init, |
62 |
3/6✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 1 times.
✗ Branch 10 not taken.
|
2 | docCommandVoid1("Initialize the entity.", |
63 | "Timestep in seconds (double)"))); | ||
64 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
1 | addCommand("getBeta", |
65 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | makeDirectGetter(*this, &m_beta, |
66 |
3/6✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 1 times.
✗ Branch 10 not taken.
|
2 | docDirectGetter("Beta parameter", "double"))); |
67 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
1 | addCommand("setBeta", |
68 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | makeCommandVoid1( |
69 | *this, &MadgwickAHRS::set_beta, | ||
70 |
3/6✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 1 times.
✗ Branch 10 not taken.
|
2 | docCommandVoid1("Set the filter parameter beta", "double"))); |
71 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
1 | addCommand("set_imu_quat", |
72 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | makeCommandVoid1( |
73 | *this, &MadgwickAHRS::set_imu_quat, | ||
74 |
3/6✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 1 times.
✗ Branch 10 not taken.
|
2 | docCommandVoid1("Set the quaternion as [w,x,y,z]", "vector"))); |
75 | 1 | } | |
76 | |||
77 | 1 | void MadgwickAHRS::init(const double &dt) { | |
78 |
1/8✗ Branch 0 not taken.
✓ Branch 1 taken 1 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 8 not taken.
✗ Branch 9 not taken.
✗ Branch 11 not taken.
✗ Branch 12 not taken.
|
1 | if (dt <= 0.0) return SEND_MSG("Timestep must be positive", MSG_TYPE_ERROR); |
79 | 1 | m_sampleFreq = 1.0 / dt; | |
80 | 1 | m_initSucceeded = true; | |
81 | } | ||
82 | |||
83 | 1 | void MadgwickAHRS::set_beta(const double &beta) { | |
84 |
2/4✓ Branch 0 taken 1 times.
✗ Branch 1 not taken.
✗ Branch 2 not taken.
✓ Branch 3 taken 1 times.
|
1 | if (beta < 0.0 || beta > 1.0) |
85 | ✗ | return SEND_MSG("Beta must be in [0,1]", MSG_TYPE_ERROR); | |
86 | 1 | m_beta = beta; | |
87 | } | ||
88 | |||
89 | ✗ | void MadgwickAHRS::set_imu_quat(const dynamicgraph::Vector &imu_quat) { | |
90 | ✗ | assert(imu_quat.size() == 4); | |
91 | ✗ | m_q0 = imu_quat[0]; | |
92 | ✗ | m_q1 = imu_quat[1]; | |
93 | ✗ | m_q2 = imu_quat[2]; | |
94 | ✗ | m_q3 = imu_quat[3]; | |
95 | } | ||
96 | |||
97 | /* ------------------------------------------------------------------- */ | ||
98 | /* --- SIGNALS ------------------------------------------------------- */ | ||
99 | /* ------------------------------------------------------------------- */ | ||
100 | |||
101 | 1 | DEFINE_SIGNAL_OUT_FUNCTION(imu_quat, dynamicgraph::Vector) { | |
102 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 1 times.
|
1 | if (!m_initSucceeded) { |
103 | ✗ | SEND_WARNING_STREAM_MSG( | |
104 | "Cannot compute signal imu_quat before initialization!"); | ||
105 | ✗ | return s; | |
106 | } | ||
107 | 1 | const dynamicgraph::Vector &accelerometer = m_accelerometerSIN(iter); | |
108 | 1 | const dynamicgraph::Vector &gyroscope = m_gyroscopeSIN(iter); | |
109 | |||
110 |
2/4✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
|
1 | getProfiler().start(PROFILE_MADGWICKAHRS_COMPUTATION); |
111 | { | ||
112 | // Update state with new measurment | ||
113 | 1 | madgwickAHRSupdateIMU(gyroscope(0), gyroscope(1), gyroscope(2), | |
114 | 1 | accelerometer(0), accelerometer(1), accelerometer(2)); | |
115 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | if (s.size() != 4) s.resize(4); |
116 | 1 | s(0) = m_q0; | |
117 | 1 | s(1) = m_q1; | |
118 | 1 | s(2) = m_q2; | |
119 | 1 | s(3) = m_q3; | |
120 | } | ||
121 |
2/4✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
|
1 | getProfiler().stop(PROFILE_MADGWICKAHRS_COMPUTATION); |
122 | |||
123 | 1 | return s; | |
124 | } | ||
125 | |||
126 | /* --- COMMANDS ------------------------------------------------------ */ | ||
127 | |||
128 | /* ------------------------------------------------------------------- */ | ||
129 | // ************************ PROTECTED MEMBER METHODS ******************** | ||
130 | /* ------------------------------------------------------------------- */ | ||
131 | |||
132 | // Fast inverse square-root | ||
133 | // See: http://en.wikipedia.org/wiki/Fast_inverse_square_root | ||
134 | 3 | double MadgwickAHRS::invSqrt(double x) { | |
135 | /* | ||
136 | float halfx = 0.5f * x; | ||
137 | float y = x; | ||
138 | long i = *(long*)&y; | ||
139 | i = 0x5f3759df - (i>>1); | ||
140 | y = *(float*)&i; | ||
141 | y = y * (1.5f - (halfx * y * y)); | ||
142 | return y;*/ | ||
143 | 3 | return (1.0 / sqrt(x)); // we're not in the 70's | |
144 | } | ||
145 | |||
146 | // IMU algorithm update | ||
147 | 1 | void MadgwickAHRS::madgwickAHRSupdateIMU(double gx, double gy, double gz, | |
148 | double ax, double ay, double az) { | ||
149 | double recipNorm; | ||
150 | double s0, s1, s2, s3; | ||
151 | double qDot1, qDot2, qDot3, qDot4; | ||
152 | double o2q0, o2q1, o2q2, o2q3, o4q0, o4q1, o4q2, o8q1, o8q2; | ||
153 | double q0q0, q1q1, q2q2, q3q3; | ||
154 | |||
155 | // Rate of change of quaternion from gyroscope | ||
156 | 1 | qDot1 = 0.5 * (-m_q1 * gx - m_q2 * gy - m_q3 * gz); | |
157 | 1 | qDot2 = 0.5 * (m_q0 * gx + m_q2 * gz - m_q3 * gy); | |
158 | 1 | qDot3 = 0.5 * (m_q0 * gy - m_q1 * gz + m_q3 * gx); | |
159 | 1 | qDot4 = 0.5 * (m_q0 * gz + m_q1 * gy - m_q2 * gx); | |
160 | |||
161 | // Compute feedback only if accelerometer measurement valid | ||
162 | // (avoids NaN in accelerometer normalisation) | ||
163 |
1/6✗ Branch 0 not taken.
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
|
1 | if (!((ax == 0.0) && (ay == 0.0) && (az == 0.0))) { |
164 | // Normalise accelerometer measurement | ||
165 | 1 | recipNorm = invSqrt(ax * ax + ay * ay + az * az); | |
166 | 1 | ax *= recipNorm; | |
167 | 1 | ay *= recipNorm; | |
168 | 1 | az *= recipNorm; | |
169 | |||
170 | // Auxiliary variables to avoid repeated arithmetic | ||
171 | 1 | o2q0 = 2.0 * m_q0; | |
172 | 1 | o2q1 = 2.0 * m_q1; | |
173 | 1 | o2q2 = 2.0 * m_q2; | |
174 | 1 | o2q3 = 2.0 * m_q3; | |
175 | 1 | o4q0 = 4.0 * m_q0; | |
176 | 1 | o4q1 = 4.0 * m_q1; | |
177 | 1 | o4q2 = 4.0 * m_q2; | |
178 | 1 | o8q1 = 8.0 * m_q1; | |
179 | 1 | o8q2 = 8.0 * m_q2; | |
180 | 1 | q0q0 = m_q0 * m_q0; | |
181 | 1 | q1q1 = m_q1 * m_q1; | |
182 | 1 | q2q2 = m_q2 * m_q2; | |
183 | 1 | q3q3 = m_q3 * m_q3; | |
184 | |||
185 | // Gradient decent algorithm corrective step | ||
186 | 1 | s0 = o4q0 * q2q2 + o2q2 * ax + o4q0 * q1q1 - o2q1 * ay; | |
187 | 1 | s1 = o4q1 * q3q3 - o2q3 * ax + 4.0 * q0q0 * m_q1 - o2q0 * ay - o4q1 + | |
188 | 1 | o8q1 * q1q1 + o8q1 * q2q2 + o4q1 * az; | |
189 | 1 | s2 = 4.0 * q0q0 * m_q2 + o2q0 * ax + o4q2 * q3q3 - o2q3 * ay - o4q2 + | |
190 | 1 | o8q2 * q1q1 + o8q2 * q2q2 + o4q2 * az; | |
191 | 1 | s3 = 4.0 * q1q1 * m_q3 - o2q1 * ax + 4.0 * q2q2 * m_q3 - o2q2 * ay; | |
192 |
2/8✓ Branch 0 taken 1 times.
✗ Branch 1 not taken.
✗ Branch 2 not taken.
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
|
1 | if (!((s0 == 0.0) && (s1 == 0.0) && (s2 == 0.0) && (s3 == 0.0))) { |
193 | // normalise step magnitude | ||
194 | 1 | recipNorm = invSqrt(s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3); | |
195 | 1 | s0 *= recipNorm; | |
196 | 1 | s1 *= recipNorm; | |
197 | 1 | s2 *= recipNorm; | |
198 | 1 | s3 *= recipNorm; | |
199 | |||
200 | // Apply feedback step | ||
201 | 1 | qDot1 -= m_beta * s0; | |
202 | 1 | qDot2 -= m_beta * s1; | |
203 | 1 | qDot3 -= m_beta * s2; | |
204 | 1 | qDot4 -= m_beta * s3; | |
205 | } | ||
206 | } | ||
207 | |||
208 | // Integrate rate of change of quaternion to yield quaternion | ||
209 | 1 | m_q0 += qDot1 * (1.0 / m_sampleFreq); | |
210 | 1 | m_q1 += qDot2 * (1.0 / m_sampleFreq); | |
211 | 1 | m_q2 += qDot3 * (1.0 / m_sampleFreq); | |
212 | 1 | m_q3 += qDot4 * (1.0 / m_sampleFreq); | |
213 | |||
214 | // Normalise quaternion | ||
215 | 1 | recipNorm = invSqrt(m_q0 * m_q0 + m_q1 * m_q1 + m_q2 * m_q2 + m_q3 * m_q3); | |
216 | 1 | m_q0 *= recipNorm; | |
217 | 1 | m_q1 *= recipNorm; | |
218 | 1 | m_q2 *= recipNorm; | |
219 | 1 | m_q3 *= recipNorm; | |
220 | 1 | } | |
221 | |||
222 | /* ------------------------------------------------------------------- */ | ||
223 | /* --- ENTITY -------------------------------------------------------- */ | ||
224 | /* ------------------------------------------------------------------- */ | ||
225 | |||
226 | ✗ | void MadgwickAHRS::display(std::ostream &os) const { | |
227 | ✗ | os << "MadgwickAHRS " << getName(); | |
228 | try { | ||
229 | ✗ | getProfiler().report_all(3, os); | |
230 | ✗ | } catch (ExceptionSignal e) { | |
231 | } | ||
232 | } | ||
233 | } // namespace sot | ||
234 | } // namespace dynamicgraph | ||
235 |