diff --git a/include/data_handling/DataNames.h b/include/data_handling/DataNames.h index 90bcf05..64d81c3 100644 --- a/include/data_handling/DataNames.h +++ b/include/data_handling/DataNames.h @@ -32,6 +32,9 @@ #define EST_VERTICAL_VELOCITY 18 #define EST_ALTITUDE 19 #define TIME_TO_APOGEE 22 +#define ROLL 25 +#define PITCH 26 +#define YAW 27 // Power #define BATTERY_VOLTAGE 20 diff --git a/include/state_estimation/OrientationEstimator.h b/include/state_estimation/OrientationEstimator.h new file mode 100644 index 0000000..17678ef --- /dev/null +++ b/include/state_estimation/OrientationEstimator.h @@ -0,0 +1,83 @@ +#ifndef ORIENTATION_ESTIMATOR_H +#define ORIENTATION_ESTIMATOR_H + +#include "data_handling/DataPoint.h" +#include "state_estimation/StateEstimationTypes.h" +#include "state_estimation/States.h" + +/** + * @brief Orientation estimator using Madgwick's algorithm for sensor fusion. + * + * This class estimates the orientation of the rocket in 3D space using data from the accelerometer, + * gyroscope, and magnetometer. When not in flight, it uses the full AHRS algorithm to fuse all three sensors. + * Once launch is detected, it relies on gyro data only for orientation updates to avoid accelerometer and magnetometer + * disturbances during flight. + */ +class OrientationEstimator { +public: + OrientationEstimator(float gainPad = 0.05f, float gainFlight = 0.005f) + : q0(1.0f), q1(0.0f), q2(0.0f), q3(0.0f), + betaPad(gainPad), betaFlight(gainFlight), + roll(0.0f), pitch(0.0f), yaw(0.0f), hasLaunched(false), lastUpdateTime(0) {} + + /** + * @brief Update the orientation estimator with new sensor data. + * This method should be called whenever new accelerometer, gyroscope, or magnetometer data is available. + * Calls updateFullAHRS or updateIMU when on pad, directly updates orientation estimate + * from gyro data only when in flight. + * @param accel Acceleration triplet (x, y, z) in m/s^2 + * @param gyro Gyroscope triplet (x, y, z) in degrees/s + * @param mag Magnetometer triplet (x, y, z) in microteslas + * @param currentTime Current timestamp in milliseconds + */ + void update(AccelerationTriplet accel, GyroTriplet gyro, MagTriplet mag, uint32_t currentTime); + + Quaternion getQuaternion() const{ + Quaternion q = {q0, q1, q2, q3}; + return q; + } + void launchDetected() { hasLaunched = true;} + float getRoll() const { return roll; } + float getPitch() const { return pitch; } + float getYaw() const { return yaw; } + +private: + float q0, q1, q2, q3; // quaternion + float betaPad; + float betaFlight; + float beta; // determines how much the algorithm relies on the accelerometer vs the gyroscope. Higher beta means more reliance on accel. + float roll, pitch, yaw; // Euler angles in degrees + bool hasLaunched; + uint32_t lastUpdateTime; // timestamp of the last update in milliseconds + + /** + * @brief Update the orientation estimate using the full AHRS algorithm (gyro + accel + mag). + * This function should never be called directly. Call update() instead, which will route to + * this or the IMU-only update as appropriate. + * @param accel Acceleration triplet (x, y, z) in m/s^2 + * @param gyro Gyroscope triplet (x, y, z) in degrees/s + * @param mag Magnetometer triplet (x, y, z) in microteslas + * @param currentTime Current timestamp in milliseconds + */ + void updateFullAHRS(AccelerationTriplet accel, GyroTriplet gyro, MagTriplet mag, uint32_t currentTime); + + /** + * @brief Update the orientation estimator using only the IMU (gyro + optional accel). + * This is used when magnetometer data is invalid. Never call directly - call update() instead. + * @param accel Acceleration triplet (x, y, z) in m/s^2 + * @param gyro Gyroscope triplet (x, y, z) in degrees/s + * @param mag Magnetometer triplet (x, y, z) in microteslas + * @param currentTime Current timestamp in milliseconds + */ + void updateIMU(AccelerationTriplet accel, GyroTriplet gyro, uint32_t currentTime); + + /* + * @brief Compute Euler angles (roll, pitch, yaw) from the current quaternion estimate. + * This is called every update to keep the Euler angles in sync with the quaternion. + * The Euler angles are in degrees and follow the aerospace convention (roll around x-axis, pitch around y-axis, yaw around z-axis). + */ + void getEuler(); +}; + + +#endif \ No newline at end of file diff --git a/include/state_estimation/StateEstimationTypes.h b/include/state_estimation/StateEstimationTypes.h index ea05daa..44235bf 100644 --- a/include/state_estimation/StateEstimationTypes.h +++ b/include/state_estimation/StateEstimationTypes.h @@ -9,4 +9,23 @@ struct AccelerationTriplet { DataPoint z; }; +struct GyroTriplet { + DataPoint x; + DataPoint y; + DataPoint z; +}; + +struct MagTriplet { + DataPoint x; + DataPoint y; + DataPoint z; +}; + +struct alignas(16) Quaternion { + float w; + float x; + float y; + float z; +}; + #endif // STATE_ESTIMATION_TYPES_H \ No newline at end of file diff --git a/src/state_estimation/OrientationEstimator.cpp b/src/state_estimation/OrientationEstimator.cpp new file mode 100644 index 0000000..efdeea0 --- /dev/null +++ b/src/state_estimation/OrientationEstimator.cpp @@ -0,0 +1,325 @@ +// NOLINTBEGIN(readability-identifier-length) +#include "state_estimation/OrientationEstimator.h" +#include + +constexpr float kLowMagThreshold = 0.01F; +constexpr float kHighMagThreshold = 10.0F; +constexpr float kOne = 1.0F; +constexpr float kTwo = 2.0F; +constexpr float kFour = 4.0F; +constexpr float kEight = 8.0F; +constexpr float kRecipTwo = 0.5F; +constexpr float kMilliToSec = 1000.0F; + + +void OrientationEstimator::update(AccelerationTriplet accel, + GyroTriplet gyro, + MagTriplet mag, + uint32_t currentTime) +{ + const float gyroX = gyro.x.data; + const float gyroY = gyro.y.data; + const float gyroZ = gyro.z.data; + + const float magX = mag.x.data; + const float magY = mag.y.data; + const float magZ = mag.z.data; + + const float dt = static_cast(currentTime - lastUpdateTime) / kMilliToSec; + + // --- Sensor usage flags --- + bool useAccel = false; + bool useMag = false; + + if (!hasLaunched){ + useAccel = true; + useMag = true; + beta = betaPad; + } + else{ + beta = betaFlight; + } + + // --- Magnetometer validity check --- + const auto magMag = static_cast(std::sqrt(magX*magX + magY*magY + magZ*magZ)); + if (magMag < kLowMagThreshold || magMag > kHighMagThreshold) { + useMag = false; + } + + // --- Route to appropriate update --- + if (!useMag) { + // IMU-only path (gyro + optional accel) + if (useAccel) { + updateIMU(accel, gyro, currentTime); + } else { + // PURE GYRO INTEGRATION (no correction) + const float qDot1 = kRecipTwo * (-q1 * gyroX - q2 * gyroY - q3 * gyroZ); + const float qDot2 = kRecipTwo * ( q0 * gyroX + q2 * gyroZ - q3 * gyroY); + const float qDot3 = kRecipTwo * ( q0 * gyroY - q1 * gyroZ + q3 * gyroX); + const float qDot4 = kRecipTwo * ( q0 * gyroZ + q1 * gyroY - q2 * gyroX); + + q0 += qDot1 * dt; + q1 += qDot2 * dt; + q2 += qDot3 * dt; + q3 += qDot4 * dt; + + const float recipNorm = kOne / static_cast(std::sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3)); + q0 *= recipNorm; + q1 *= recipNorm; + q2 *= recipNorm; + q3 *= recipNorm; + + getEuler(); + lastUpdateTime = currentTime; + } + return; + } + + // used when on pad + updateFullAHRS(accel, gyro, mag, currentTime); +} + +void OrientationEstimator::updateFullAHRS(AccelerationTriplet accel, GyroTriplet gyro, MagTriplet mag, uint32_t currentTime){ + float accelX = accel.x.data; + float accelY = accel.y.data; + float accelZ = accel.z.data; + + const float gyroX = gyro.x.data; + const float gyroY = gyro.y.data; + const float gyroZ = gyro.z.data; + + float magX = mag.x.data; + float magY = mag.y.data; + float magZ = mag.z.data; + + float recipNorm; + + // gradient descent algorithm corrective step variables + float s0; + float s1; + float s2; + float s3; + float qDot1; + float qDot2; + float qDot3; + float qDot4; + + float hx; + float hy; + 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) + + + const float dt = static_cast(currentTime - lastUpdateTime) / kMilliToSec; // convert ms to seconds + + // Use IMU algorithm if magnetometer measurement invalid (avoids NaN in magnetometer normalisation) + if((magX == 0.0F) && (magY == 0.0F) && (magZ == 0.0F)) { + updateIMU(accel, gyro, currentTime); + return; + } + + // Rate of change of quaternion from gyroscope + qDot1 = kRecipTwo * (-q1 * gyroX - q2 * gyroY - q3 * gyroZ); + qDot2 = kRecipTwo * (q0 * gyroX + q2 * gyroZ - q3 * gyroY); + qDot3 = kRecipTwo * (q0 * gyroY - q1 * gyroZ + q3 * gyroX); + qDot4 = kRecipTwo * (q0 * gyroZ + q1 * gyroY - q2 * gyroX); + + // Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation) + if((accelX != 0.0F) || (accelY != 0.0F) || (accelZ != 0.0F)) { + + // Normalise accelerometer measurement + recipNorm = 1 / std::sqrt(accelX * accelX + accelY * accelY + accelZ * accelZ); + accelX *= recipNorm; + accelY *= recipNorm; + accelZ *= recipNorm; + + // Normalise magnetometer measurement + recipNorm = 1 /std::sqrt(magX * magX + magY * magY + magZ * magZ); + magX *= recipNorm; + magY *= recipNorm; + magZ *= recipNorm; + + // Auxiliary variables to avoid repeated arithmetic + _2q0magX = kTwo * q0 * magX; + _2q0magY = kTwo * q0 * magY; + _2q0magZ = kTwo * q0 * magZ; + _2q1magX = kTwo * q1 * magX; + _2q0 = kTwo * q0; + _2q1 = kTwo * q1; + _2q2 = kTwo * q2; + _2q3 = kTwo * q3; + _2q0q2 = kTwo * q0 * q2; + _2q2q3 = kTwo * 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 Earth's magnetic field + hx = magX * q0q0 - _2q0magY * q3 + _2q0magZ * q2 + magX * q1q1 + _2q1 * magY * q2 + _2q1 * magZ * q3 - magX * q2q2 - magX * q3q3; + hy = _2q0magX * q3 + magY * q0q0 - _2q0magZ * q1 + _2q1magX * q2 - magY * q1q1 + magY * q2q2 + _2q2 * magZ * q3 - magY * q3q3; + _2bx = static_cast(std::sqrt(hx * hx + hy * hy)); + _2bz = -_2q0magX * q2 + _2q0magY * q1 + magZ * q0q0 + _2q1magX * q3 - magZ * q1q1 + _2q2 * magY * q3 - magZ * q2q2 + magZ * q3q3; + _4bx = kTwo * _2bx; + _4bz = kTwo * _2bz; + + // Gradient decent algorithm corrective step + 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); + 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); + 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); + 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); + recipNorm = 1 / std::sqrt(s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3); // normalise 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 * dt; + q1 += qDot2 * dt; + q2 += qDot3 * dt; + q3 += qDot4 * dt; + + // Normalise quaternion + recipNorm = 1 / std::sqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3); + q0 *= recipNorm; + q1 *= recipNorm; + q2 *= recipNorm; + q3 *= recipNorm; + + getEuler(); + lastUpdateTime = currentTime; +} + +void OrientationEstimator::updateIMU(AccelerationTriplet accel, GyroTriplet gyro, uint32_t currentTime){ + float accelX = accel.x.data; + float accelY = accel.y.data; + float accelZ = accel.z.data; + + const float gyroX = gyro.x.data; + const float gyroY = gyro.y.data; + const float gyroZ = gyro.z.data; + + float recipNorm; + + // gradient descent algorithm corrective step variables + float s0; + float s1; + float s2; + float s3; + float qDot1; + float qDot2; + float qDot3; + float qDot4; + + float _2q0, _2q1, _2q2, _2q3, _4q0, _4q1, _4q2 ,_8q1, _8q2, q0q0, q1q1, q2q2, q3q3; //NOLINT(readability-isolate-declaration) + + const float dt = static_cast(currentTime - lastUpdateTime) / kMilliToSec; // convert ms to seconds + + // Rate of change of quaternion from gyroscope + qDot1 = kRecipTwo * (-q1 * gyroX - q2 * gyroY - q3 * gyroZ); + qDot2 = kRecipTwo * (q0 * gyroX + q2 * gyroZ - q3 * gyroY); + qDot3 = kRecipTwo * (q0 * gyroY - q1 * gyroZ + q3 * gyroX); + qDot4 = kRecipTwo * (q0 * gyroZ + q1 * gyroY - q2 * gyroX); + + // Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation) + if((accelX != 0.0F) || (accelY != 0.0F) || (accelZ != 0.0F)) { + + // Normalise accelerometer measurement + recipNorm = 1 / std::sqrt(accelX * accelX + accelY * accelY + accelZ * accelZ); + accelX *= recipNorm; + accelY *= recipNorm; + accelZ *= recipNorm; + + // Auxiliary variables to avoid repeated arithmetic + _2q0 = kTwo * q0; + _2q1 = kTwo * q1; + _2q2 = kTwo * q2; + _2q3 = kTwo * q3; + _4q0 = kFour * q0; + _4q1 = kFour * q1; + _4q2 = kFour * q2; + _8q1 = kEight * q1; + _8q2 = kEight * q2; + q0q0 = q0 * q0; + q1q1 = q1 * q1; + q2q2 = q2 * q2; + q3q3 = q3 * q3; + + // Gradient decent algorithm corrective step + s0 = _4q0 * q2q2 + _2q2 * accelX + _4q0 * q1q1 - _2q1 * accelY; + s1 = _4q1 * q3q3 - _2q3 * accelX + kFour * q0q0 * q1 - _2q0 * accelY - _4q1 + _8q1 * q1q1 + _8q1 * q2q2 + _4q1 * accelZ; + s2 = kFour * q0q0 * q2 + _2q0 * accelX + _4q2 * q3q3 - _2q3 * accelY - _4q2 + _8q2 * q1q1 + _8q2 * q2q2 + _4q2 * accelZ; + s3 = kFour * q1q1 * q3 - _2q1 * accelX + kFour * q2q2 * q3 - _2q2 * accelY; + recipNorm = 1 / std::sqrt(s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3); // normalise 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 * dt; + q1 += qDot2 * dt; + q2 += qDot3 * dt; + q3 += qDot4 * dt; + + // Normalise quaternion + recipNorm = 1 / std::sqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3); + q0 *= recipNorm; + q1 *= recipNorm; + q2 *= recipNorm; + q3 *= recipNorm; + + getEuler(); + lastUpdateTime = currentTime; +} + +void OrientationEstimator::getEuler() +{ + // Roll (x-accelXis rotation) + roll = static_cast(std::atan2( + kTwo * (q0 * q1 + q2 * q3), + kOne - kTwo * (q1 * q1 + q2 * q2) + )); + + // Pitch (y-accelXis rotation) + float t = kTwo * (q0 * q2 - q3 * q1); + if (t > kOne){ + t = kOne; + } + if (t < -kOne){ + t = -kOne; + } + pitch = static_cast(std::asin(t)); + + // Yaw (z-accelXis rotation) + yaw = static_cast(std::atan2( + kTwo * (q0 * q3 + q1 * q2), + kOne - kTwo * (q2 * q2 + q3 * q3) + )); + + const float rad2deg = 57.29577951308232F; + roll *= rad2deg; + pitch *= rad2deg; + yaw *= rad2deg; +} +// NOLINTEND(readability-identifier-length) \ No newline at end of file diff --git a/test/test_orientation_estimator/test_orientation_estimator.cpp b/test/test_orientation_estimator/test_orientation_estimator.cpp new file mode 100644 index 0000000..6e65f64 --- /dev/null +++ b/test/test_orientation_estimator/test_orientation_estimator.cpp @@ -0,0 +1,81 @@ +#define DEBUG +#include +#include +#include +#include +#include +#include +#include "unity.h" +#include "state_estimation/OrientationEstimator.h" +#include "data_handling/DataPoint.h" +#include "ArduinoHAL.h" +#include "state_estimation/StateEstimationTypes.h" +#include "../CSVMockData.h" + +void setUp(void) { + Serial.clear(); +} + +void tearDown(void) { + Serial.clear(); +} + +void test_orientation_estimator_with_real_data(void) { + // Create CSV provider to read test data with 25Hz sample rate (40ms interval) + // download data/AA Data Collection - Second Launch Trimmed.csv + CSVDataProvider provider("data/AA Data Collection - Second Launch Trimmed.csv", 25.0f); + OrientationEstimator estimator; + + + std::ofstream csv("orientation_test_output.csv"); + if (!csv.is_open()) { + std::cerr << "Failed to open output CSV file" << std::endl; + TEST_FAIL(); + return; + } + csv << "time,accelx,accely,accelz,gyrox,gyroy,gyroz,magx,magy,magz,roll,pitch,yaw\n"; + + while (provider.hasNextDataPoint()) { + SensorData data = provider.getNextDataPoint(); + + // Create sensor triplets from the data point + AccelerationTriplet accel = { + DataPoint(data.time, data.accelx), + DataPoint(data.time, data.accely), + DataPoint(data.time, data.accelz) + }; + GyroTriplet gyro = { + DataPoint(data.time, data.gyrox), + DataPoint(data.time, data.gyroy), + DataPoint(data.time, data.gyroz) + }; + MagTriplet mag = { + DataPoint(data.time, data.magx), + DataPoint(data.time, data.magy), + DataPoint(data.time, data.magz) + }; + + // Update the orientation estimator with the new sensor data + estimator.update(accel, gyro, mag, data.time); + + // Write results to CSV + csv << data.time << "," + << accel.x.data << "," << accel.y.data << "," << accel.z.data << "," + << gyro.x.data << "," << gyro.y.data << "," << gyro.z.data << "," + << mag.x.data << "," << mag.y.data << "," << mag.z.data << "," + << estimator.getRoll() << "," + << estimator.getPitch() << "," + << estimator.getYaw() << "\n"; + + TEST_ASSERT_TRUE(-180.0f <= estimator.getRoll() && estimator.getRoll() <= 180.0f); + TEST_ASSERT_TRUE(-180.0f <= estimator.getPitch() && estimator.getPitch() <= 180.0f); + TEST_ASSERT_TRUE(-180.0f <= estimator.getYaw() && estimator.getYaw() <= 180.0f); + } +} + +int main() { + UNITY_BEGIN(); + RUN_TEST(test_orientation_estimator_with_real_data); + return UNITY_END(); +} +