Avionics
Core avionics package for CURE flight computers
Loading...
Searching...
No Matches
OrientationEstimator.cpp
Go to the documentation of this file.
1// NOLINTBEGIN(readability-identifier-length)
2#include "state_estimation/OrientationEstimator.h"
3#include <cmath>
4
5constexpr float kLowMagThreshold = 0.01F;
6constexpr float kHighMagThreshold = 10.0F;
7constexpr float kOne = 1.0F;
8constexpr float kTwo = 2.0F;
9constexpr float kFour = 4.0F;
10constexpr float kEight = 8.0F;
11constexpr float kRecipTwo = 0.5F;
12constexpr float kMilliToSec = 1000.0F;
13
14
15void OrientationEstimator::update(AccelerationTriplet accel,
16 GyroTriplet gyro,
17 MagTriplet mag,
18 uint32_t currentTime)
19{
20 const float gyroX = gyro.x.data;
21 const float gyroY = gyro.y.data;
22 const float gyroZ = gyro.z.data;
23
24 const float magX = mag.x.data;
25 const float magY = mag.y.data;
26 const float magZ = mag.z.data;
27
28 const float dt = static_cast<float>(currentTime - lastUpdateTime) / kMilliToSec;
29
30 // --- Sensor usage flags ---
31 bool useAccel = false;
32 bool useMag = false;
33
34 if (!hasLaunched){
35 useAccel = true;
36 useMag = true;
37 beta = betaPad;
38 }
39 else{
40 beta = betaFlight;
41 }
42
43 // --- Magnetometer validity check ---
44 const auto magMag = static_cast<float>(std::sqrt(magX*magX + magY*magY + magZ*magZ));
45 if (magMag < kLowMagThreshold || magMag > kHighMagThreshold) {
46 useMag = false;
47 }
48
49 // --- Route to appropriate update ---
50 if (!useMag) {
51 // IMU-only path (gyro + optional accel)
52 if (useAccel) {
53 updateIMU(accel, gyro, currentTime);
54 } else {
55 // PURE GYRO INTEGRATION (no correction)
56 const float qDot1 = kRecipTwo * (-q1 * gyroX - q2 * gyroY - q3 * gyroZ);
57 const float qDot2 = kRecipTwo * ( q0 * gyroX + q2 * gyroZ - q3 * gyroY);
58 const float qDot3 = kRecipTwo * ( q0 * gyroY - q1 * gyroZ + q3 * gyroX);
59 const float qDot4 = kRecipTwo * ( q0 * gyroZ + q1 * gyroY - q2 * gyroX);
60
61 q0 += qDot1 * dt;
62 q1 += qDot2 * dt;
63 q2 += qDot3 * dt;
64 q3 += qDot4 * dt;
65
66 const float recipNorm = kOne / static_cast<float>(std::sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3));
67 q0 *= recipNorm;
68 q1 *= recipNorm;
69 q2 *= recipNorm;
70 q3 *= recipNorm;
71
72 getEuler();
73 lastUpdateTime = currentTime;
74 }
75 return;
76 }
77
78 // used when on pad
79 updateFullAHRS(accel, gyro, mag, currentTime);
80}
81
82void OrientationEstimator::updateFullAHRS(AccelerationTriplet accel, GyroTriplet gyro, MagTriplet mag, uint32_t currentTime){
83 float accelX = accel.x.data;
84 float accelY = accel.y.data;
85 float accelZ = accel.z.data;
86
87 const float gyroX = gyro.x.data;
88 const float gyroY = gyro.y.data;
89 const float gyroZ = gyro.z.data;
90
91 float magX = mag.x.data;
92 float magY = mag.y.data;
93 float magZ = mag.z.data;
94
95 float recipNorm;
96
97 // gradient descent algorithm corrective step variables
98 float s0;
99 float s1;
100 float s2;
101 float s3;
102 float qDot1;
103 float qDot2;
104 float qDot3;
105 float qDot4;
106
107 float hx;
108 float hy;
109 float _2q0magX, _2q0magY, _2q0magZ, _2q1magX, _2bx, _2bz, _4bx, _4bz, _2q0, _2q1, _2q2, _2q3, _2q0q2, _2q2q3, q0q0, q0q1, q0q2, q0q3, q1q1, q1q2, q1q3, q2q2, q2q3, q3q3; //NOLINT(readability-isolate-declaration)
110
111
112 const float dt = static_cast<float>(currentTime - lastUpdateTime) / kMilliToSec; // convert ms to seconds
113
114 // Use IMU algorithm if magnetometer measurement invalid (avoids NaN in magnetometer normalisation)
115 if((magX == 0.0F) && (magY == 0.0F) && (magZ == 0.0F)) {
116 updateIMU(accel, gyro, currentTime);
117 return;
118 }
119
120 // Rate of change of quaternion from gyroscope
121 qDot1 = kRecipTwo * (-q1 * gyroX - q2 * gyroY - q3 * gyroZ);
122 qDot2 = kRecipTwo * (q0 * gyroX + q2 * gyroZ - q3 * gyroY);
123 qDot3 = kRecipTwo * (q0 * gyroY - q1 * gyroZ + q3 * gyroX);
124 qDot4 = kRecipTwo * (q0 * gyroZ + q1 * gyroY - q2 * gyroX);
125
126 // Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation)
127 if((accelX != 0.0F) || (accelY != 0.0F) || (accelZ != 0.0F)) {
128
129 // Normalise accelerometer measurement
130 recipNorm = 1 / std::sqrt(accelX * accelX + accelY * accelY + accelZ * accelZ);
131 accelX *= recipNorm;
132 accelY *= recipNorm;
133 accelZ *= recipNorm;
134
135 // Normalise magnetometer measurement
136 recipNorm = 1 /std::sqrt(magX * magX + magY * magY + magZ * magZ);
137 magX *= recipNorm;
138 magY *= recipNorm;
139 magZ *= recipNorm;
140
141 // Auxiliary variables to avoid repeated arithmetic
142 _2q0magX = kTwo * q0 * magX;
143 _2q0magY = kTwo * q0 * magY;
144 _2q0magZ = kTwo * q0 * magZ;
145 _2q1magX = kTwo * q1 * magX;
146 _2q0 = kTwo * q0;
147 _2q1 = kTwo * q1;
148 _2q2 = kTwo * q2;
149 _2q3 = kTwo * q3;
150 _2q0q2 = kTwo * q0 * q2;
151 _2q2q3 = kTwo * q2 * q3;
152 q0q0 = q0 * q0;
153 q0q1 = q0 * q1;
154 q0q2 = q0 * q2;
155 q0q3 = q0 * q3;
156 q1q1 = q1 * q1;
157 q1q2 = q1 * q2;
158 q1q3 = q1 * q3;
159 q2q2 = q2 * q2;
160 q2q3 = q2 * q3;
161 q3q3 = q3 * q3;
162
163 // Reference direction of Earth's magnetic field
164 hx = magX * q0q0 - _2q0magY * q3 + _2q0magZ * q2 + magX * q1q1 + _2q1 * magY * q2 + _2q1 * magZ * q3 - magX * q2q2 - magX * q3q3;
165 hy = _2q0magX * q3 + magY * q0q0 - _2q0magZ * q1 + _2q1magX * q2 - magY * q1q1 + magY * q2q2 + _2q2 * magZ * q3 - magY * q3q3;
166 _2bx = static_cast<float>(std::sqrt(hx * hx + hy * hy));
167 _2bz = -_2q0magX * q2 + _2q0magY * q1 + magZ * q0q0 + _2q1magX * q3 - magZ * q1q1 + _2q2 * magY * q3 - magZ * q2q2 + magZ * q3q3;
168 _4bx = kTwo * _2bx;
169 _4bz = kTwo * _2bz;
170
171 // Gradient decent algorithm corrective step
172 s0 = -_2q2 * (kTwo * q1q3 - _2q0q2 - accelX) + _2q1 * (kTwo * q0q1 + _2q2q3 - accelY) - _2bz * q2 * (_2bx * (kRecipTwo - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - magX) + (-_2bx * q3 + _2bz * q1) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - magY) + _2bx * q2 * (_2bx * (q0q2 + q1q3) + _2bz * (kRecipTwo - q1q1 - q2q2) - magZ);
173 s1 = _2q3 * (kTwo * q1q3 - _2q0q2 - accelX) + _2q0 * (kTwo * q0q1 + _2q2q3 - accelY) - kFour * q1 * (1 - kTwo * q1q1 - kTwo * q2q2 - accelZ) + _2bz * q3 * (_2bx * (kRecipTwo - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - magX) + (_2bx * q2 + _2bz * q0) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - magY) + (_2bx * q3 - _4bz * q1) * (_2bx * (q0q2 + q1q3) + _2bz * (kRecipTwo - q1q1 - q2q2) - magZ);
174 s2 = -_2q0 * (kTwo * q1q3 - _2q0q2 - accelX) + _2q3 * (kTwo * q0q1 + _2q2q3 - accelY) - kFour * q2 * (1 - kTwo * q1q1 - kTwo * q2q2 - accelZ) + (-_4bx * q2 - _2bz * q0) * (_2bx * (kRecipTwo - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - magX) + (_2bx * q1 + _2bz * q3) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - magY) + (_2bx * q0 - _4bz * q2) * (_2bx * (q0q2 + q1q3) + _2bz * (kRecipTwo - q1q1 - q2q2) - magZ);
175 s3 = _2q1 * (kTwo * q1q3 - _2q0q2 - accelX) + _2q2 * (kTwo * q0q1 + _2q2q3 - accelY) + (-_4bx * q3 + _2bz * q1) * (_2bx * (kRecipTwo - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - magX) + (-_2bx * q0 + _2bz * q2) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - magY) + _2bx * q1 * (_2bx * (q0q2 + q1q3) + _2bz * (kRecipTwo - q1q1 - q2q2) - magZ);
176 recipNorm = 1 / std::sqrt(s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3); // normalise step magnitude
177 s0 *= recipNorm;
178 s1 *= recipNorm;
179 s2 *= recipNorm;
180 s3 *= recipNorm;
181
182 // Apply feedback step
183 qDot1 -= beta * s0;
184 qDot2 -= beta * s1;
185 qDot3 -= beta * s2;
186 qDot4 -= beta * s3;
187 }
188
189 // Integrate rate of change of quaternion to yield quaternion
190 q0 += qDot1 * dt;
191 q1 += qDot2 * dt;
192 q2 += qDot3 * dt;
193 q3 += qDot4 * dt;
194
195 // Normalise quaternion
196 recipNorm = 1 / std::sqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
197 q0 *= recipNorm;
198 q1 *= recipNorm;
199 q2 *= recipNorm;
200 q3 *= recipNorm;
201
202 getEuler();
203 lastUpdateTime = currentTime;
204}
205
206void OrientationEstimator::updateIMU(AccelerationTriplet accel, GyroTriplet gyro, uint32_t currentTime){
207 float accelX = accel.x.data;
208 float accelY = accel.y.data;
209 float accelZ = accel.z.data;
210
211 const float gyroX = gyro.x.data;
212 const float gyroY = gyro.y.data;
213 const float gyroZ = gyro.z.data;
214
215 float recipNorm;
216
217 // gradient descent algorithm corrective step variables
218 float s0;
219 float s1;
220 float s2;
221 float s3;
222 float qDot1;
223 float qDot2;
224 float qDot3;
225 float qDot4;
226
227 float _2q0, _2q1, _2q2, _2q3, _4q0, _4q1, _4q2 ,_8q1, _8q2, q0q0, q1q1, q2q2, q3q3; //NOLINT(readability-isolate-declaration)
228
229 const float dt = static_cast<float>(currentTime - lastUpdateTime) / kMilliToSec; // convert ms to seconds
230
231 // Rate of change of quaternion from gyroscope
232 qDot1 = kRecipTwo * (-q1 * gyroX - q2 * gyroY - q3 * gyroZ);
233 qDot2 = kRecipTwo * (q0 * gyroX + q2 * gyroZ - q3 * gyroY);
234 qDot3 = kRecipTwo * (q0 * gyroY - q1 * gyroZ + q3 * gyroX);
235 qDot4 = kRecipTwo * (q0 * gyroZ + q1 * gyroY - q2 * gyroX);
236
237 // Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation)
238 if((accelX != 0.0F) || (accelY != 0.0F) || (accelZ != 0.0F)) {
239
240 // Normalise accelerometer measurement
241 recipNorm = 1 / std::sqrt(accelX * accelX + accelY * accelY + accelZ * accelZ);
242 accelX *= recipNorm;
243 accelY *= recipNorm;
244 accelZ *= recipNorm;
245
246 // Auxiliary variables to avoid repeated arithmetic
247 _2q0 = kTwo * q0;
248 _2q1 = kTwo * q1;
249 _2q2 = kTwo * q2;
250 _2q3 = kTwo * q3;
251 _4q0 = kFour * q0;
252 _4q1 = kFour * q1;
253 _4q2 = kFour * q2;
254 _8q1 = kEight * q1;
255 _8q2 = kEight * q2;
256 q0q0 = q0 * q0;
257 q1q1 = q1 * q1;
258 q2q2 = q2 * q2;
259 q3q3 = q3 * q3;
260
261 // Gradient decent algorithm corrective step
262 s0 = _4q0 * q2q2 + _2q2 * accelX + _4q0 * q1q1 - _2q1 * accelY;
263 s1 = _4q1 * q3q3 - _2q3 * accelX + kFour * q0q0 * q1 - _2q0 * accelY - _4q1 + _8q1 * q1q1 + _8q1 * q2q2 + _4q1 * accelZ;
264 s2 = kFour * q0q0 * q2 + _2q0 * accelX + _4q2 * q3q3 - _2q3 * accelY - _4q2 + _8q2 * q1q1 + _8q2 * q2q2 + _4q2 * accelZ;
265 s3 = kFour * q1q1 * q3 - _2q1 * accelX + kFour * q2q2 * q3 - _2q2 * accelY;
266 recipNorm = 1 / std::sqrt(s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3); // normalise step magnitude
267 s0 *= recipNorm;
268 s1 *= recipNorm;
269 s2 *= recipNorm;
270 s3 *= recipNorm;
271
272 // Apply feedback step
273 qDot1 -= beta * s0;
274 qDot2 -= beta * s1;
275 qDot3 -= beta * s2;
276 qDot4 -= beta * s3;
277 }
278
279 // Integrate rate of change of quaternion to yield quaternion
280 q0 += qDot1 * dt;
281 q1 += qDot2 * dt;
282 q2 += qDot3 * dt;
283 q3 += qDot4 * dt;
284
285 // Normalise quaternion
286 recipNorm = 1 / std::sqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
287 q0 *= recipNorm;
288 q1 *= recipNorm;
289 q2 *= recipNorm;
290 q3 *= recipNorm;
291
292 getEuler();
293 lastUpdateTime = currentTime;
294}
295
296void OrientationEstimator::getEuler()
297{
298 // Roll (x-accelXis rotation)
299 roll = static_cast<float>(std::atan2(
300 kTwo * (q0 * q1 + q2 * q3),
301 kOne - kTwo * (q1 * q1 + q2 * q2)
302 ));
303
304 // Pitch (y-accelXis rotation)
305 float t = kTwo * (q0 * q2 - q3 * q1);
306 if (t > kOne){
307 t = kOne;
308 }
309 if (t < -kOne){
310 t = -kOne;
311 }
312 pitch = static_cast<float>(std::asin(t));
313
314 // Yaw (z-accelXis rotation)
315 yaw = static_cast<float>(std::atan2(
316 kTwo * (q0 * q3 + q1 * q2),
317 kOne - kTwo * (q2 * q2 + q3 * q3)
318 ));
319
320 const float rad2deg = 57.29577951308232F;
321 roll *= rad2deg;
322 pitch *= rad2deg;
323 yaw *= rad2deg;
324}
325// NOLINTEND(readability-identifier-length)
kOne
constexpr float kOne
Definition OrientationEstimator.cpp:7
kLowMagThreshold
constexpr float kLowMagThreshold
Definition OrientationEstimator.cpp:5
kMilliToSec
constexpr float kMilliToSec
Definition OrientationEstimator.cpp:12
kHighMagThreshold
constexpr float kHighMagThreshold
Definition OrientationEstimator.cpp:6
kRecipTwo
constexpr float kRecipTwo
Definition OrientationEstimator.cpp:11
kFour
constexpr float kFour
Definition OrientationEstimator.cpp:9
kEight
constexpr float kEight
Definition OrientationEstimator.cpp:10
kTwo
constexpr float kTwo
Definition OrientationEstimator.cpp:8
DataPoint::data
float data
Definition DataPoint.h:14
OrientationEstimator::update
void update(AccelerationTriplet accel, GyroTriplet gyro, MagTriplet mag, uint32_t currentTime)
Update the orientation estimator with new sensor data. This method should be called whenever new acce...
Definition OrientationEstimator.cpp:15