From 35bccefe82e86407aeeda3bef1476683543c27c7 Mon Sep 17 00:00:00 2001 From: Bjorn Date: Fri, 24 Jul 2026 15:40:46 -0700 Subject: [PATCH 1/7] Start Mahony attitude filter with gyro propagation --- mahony/mahony.c | 186 +++++++++++++ mahony/mahony.h | 112 ++++++++ math_sdr/math_sdr.h | 4 +- test/mahony/.gitignore | 6 + test/mahony/Makefile | 106 +++++++ test/mahony/main.h | 8 + test/mahony/test_mahony.c | 562 ++++++++++++++++++++++++++++++++++++++ 7 files changed, 983 insertions(+), 1 deletion(-) create mode 100644 mahony/mahony.c create mode 100644 mahony/mahony.h create mode 100644 test/mahony/.gitignore create mode 100644 test/mahony/Makefile create mode 100644 test/mahony/main.h create mode 100644 test/mahony/test_mahony.c diff --git a/mahony/mahony.c b/mahony/mahony.c new file mode 100644 index 0000000..cc6c9b3 --- /dev/null +++ b/mahony/mahony.c @@ -0,0 +1,186 @@ +/******************************************************************************* + * + * FILE: + * mahony.c + * + * DESCRIPTION: + * Mahony attitude filter implementation. + * + ******************************************************************************/ + +/*------------------------------------------------------------------------------ + Standard Includes + ------------------------------------------------------------------------------*/ +#include +#include + +/*------------------------------------------------------------------------------ + Project Includes + ------------------------------------------------------------------------------*/ +#include "mahony.h" + +/*------------------------------------------------------------------------------ + Private Functions + ------------------------------------------------------------------------------*/ + +/** + * @brief Determines whether every quaternion component is finite. + */ +static bool mahony_quat_is_finite + ( + QUAT quaternion + ) +{ +return + ( + isfinite(quaternion.w) && + isfinite(quaternion.x) && + isfinite(quaternion.y) && + isfinite(quaternion.z) + ); + +} /* mahony_quat_is_finite */ + + +/** + * @brief Determines whether every vector component is finite. + */ +static bool mahony_vector_is_finite + ( + VECTOR_3F vector + ) +{ +return + ( + isfinite(vector.x) && + isfinite(vector.y) && + isfinite(vector.z) + ); + +} /* mahony_vector_is_finite */ + + +/*------------------------------------------------------------------------------ + Public Functions + ------------------------------------------------------------------------------*/ + +bool mahony_init + ( + MAHONY_FILTER *filter, + QUAT initial_attitude, + float proportional_gain, + float integral_gain + ) +{ +if ( filter == NULL ) + { + return false; + } + +if ( !mahony_quat_is_finite(initial_attitude) ) + { + return false; + } + +if ( !isfinite(proportional_gain) || + !isfinite(integral_gain) ) + { + return false; + } + +if ( proportional_gain < 0.0f || + integral_gain < 0.0f ) + { + return false; + } + +filter->attitude = quat_normalize(initial_attitude); + +filter->integral_error.x = 0.0f; +filter->integral_error.y = 0.0f; +filter->integral_error.z = 0.0f; + +filter->proportional_gain = proportional_gain; +filter->integral_gain = integral_gain; + +return true; + +} /* mahony_init */ + + +bool mahony_update_gyro + ( + MAHONY_FILTER *filter, + VECTOR_3F gyro_body_rad_s, + float delta_time_s + ) +{ +QUAT angular_velocity; +QUAT attitude_derivative; +QUAT attitude_delta; + +if ( filter == NULL ) + { + return false; + } + +if ( !mahony_quat_is_finite(filter->attitude) ) + { + return false; + } + +if ( !mahony_vector_is_finite(gyro_body_rad_s) ) + { + return false; + } + +if ( !isfinite(delta_time_s) || + delta_time_s <= 0.0f ) + { + return false; + } + +/* + * The attitude quaternion is a body-to-world rotation and angular velocity is + * expressed in the body frame: + * + * q_dot = 0.5 * q * omega_body + */ +angular_velocity.w = 0.0f; +angular_velocity.x = gyro_body_rad_s.x; +angular_velocity.y = gyro_body_rad_s.y; +angular_velocity.z = gyro_body_rad_s.z; + +attitude_derivative = quat_mult + ( + filter->attitude, + angular_velocity + ); + +attitude_derivative = quat_scale + ( + attitude_derivative, + 0.5f + ); + +attitude_delta = quat_scale + ( + attitude_derivative, + delta_time_s + ); + +filter->attitude = quat_add + ( + filter->attitude, + attitude_delta + ); + +filter->attitude = quat_normalize(filter->attitude); + +return true; + +} /* mahony_update_gyro */ + +/******************************************************************************* + * END OF FILE + ******************************************************************************/ \ No newline at end of file diff --git a/mahony/mahony.h b/mahony/mahony.h new file mode 100644 index 0000000..d56cac5 --- /dev/null +++ b/mahony/mahony.h @@ -0,0 +1,112 @@ +/******************************************************************************* + * + * FILE: + * mahony.h + * + * DESCRIPTION: + * Mahony attitude filter interface. + * + ******************************************************************************/ + +#ifndef MAHONY_H +#define MAHONY_H + +#ifdef __cplusplus +extern "C" +{ +#endif + +/*------------------------------------------------------------------------------ + Standard Includes + ------------------------------------------------------------------------------*/ +#include + +/*------------------------------------------------------------------------------ + Project Includes + ------------------------------------------------------------------------------*/ +#include "math_sdr.h" + +/*------------------------------------------------------------------------------ + Typedefs + ------------------------------------------------------------------------------*/ + +/** + * @brief Three-dimensional floating-point vector. + */ +typedef struct _VECTOR_3F + { + float x; + float y; + float z; + } VECTOR_3F; + +/** + * @brief State and gains for a Mahony attitude filter. + */ +typedef struct _MAHONY_FILTER + { + /** + * Body-to-world attitude quaternion. + */ + QUAT attitude; + + /** + * Accumulated attitude error used for integral gyro-bias correction. + */ + VECTOR_3F integral_error; + + float proportional_gain; + float integral_gain; + + } MAHONY_FILTER; + +/*------------------------------------------------------------------------------ + Function Prototypes + ------------------------------------------------------------------------------*/ + +/** + * @brief Initializes a Mahony attitude filter. + * + * @param filter Filter instance to initialize. + * @param initial_attitude Initial body-to-world attitude quaternion. + * @param proportional_gain Proportional correction gain. + * @param integral_gain Integral correction gain. + * + * @return true when initialization succeeds; otherwise false. + */ +bool mahony_init + ( + MAHONY_FILTER *filter, + QUAT initial_attitude, + float proportional_gain, + float integral_gain + ); + +/** + * @brief Propagates attitude using body-frame gyroscope measurements. + * + * The attitude quaternion represents the body-to-world rotation. The angular + * velocity vector must be expressed in the body frame in radians per second. + * + * @param filter Initialized filter instance. + * @param gyro_body_rad_s Body-frame angular velocity in radians per second. + * @param delta_time_s Elapsed time in seconds. + * + * @return true when the attitude was updated; otherwise false. + */ +bool mahony_update_gyro + ( + MAHONY_FILTER *filter, + VECTOR_3F gyro_body_rad_s, + float delta_time_s + ); + +#ifdef __cplusplus +} +#endif + +#endif /* MAHONY_H */ + +/******************************************************************************* + * END OF FILE + ******************************************************************************/ \ No newline at end of file diff --git a/math_sdr/math_sdr.h b/math_sdr/math_sdr.h index 37b594a..a884f2a 100644 --- a/math_sdr/math_sdr.h +++ b/math_sdr/math_sdr.h @@ -41,8 +41,10 @@ extern "C" { /*------------------------------------------------------------------------------ Includes ------------------------------------------------------------------------------*/ -#include #include +#include +#include +#include /*------------------------------------------------------------------------------ diff --git a/test/mahony/.gitignore b/test/mahony/.gitignore new file mode 100644 index 0000000..9bf4711 --- /dev/null +++ b/test/mahony/.gitignore @@ -0,0 +1,6 @@ +build/ +coverage/ +results.txt +*.gcda +*.gcno +*.gcov diff --git a/test/mahony/Makefile b/test/mahony/Makefile new file mode 100644 index 0000000..42a4902 --- /dev/null +++ b/test/mahony/Makefile @@ -0,0 +1,106 @@ +################################################################ +# +# math_sdr unit tests (based on gcc) +# +################################################################ + +################################################################ +# target +################################################################ +TARGET = mahony + +################################################################ +# build variables +################################################################ +DEBUG ?= 0 +OPT = -Og + +################################################################ +# paths +################################################################ +BUILD_DIR = build +ROOT_DIR = ../.. + +################################################################ +# source +################################################################ +TEST_SOURCES = \ +test_mahony.c + +COV_C_SOURCES = \ +$(ROOT_DIR)/mahony/mahony.c \ +$(ROOT_DIR)/math_sdr/math_sdr.c + +FRAMEWORK_SOURCES = \ +$(ROOT_DIR)/test/framework/src/test_assert.c \ +$(ROOT_DIR)/test/framework/src/test_runner.c + +C_SOURCES = $(TEST_SOURCES) $(COV_C_SOURCES) $(FRAMEWORK_SOURCES) + +################################################################ +# compiler +################################################################ +CC = gcc + +################################################################ +# C flags +################################################################ +C_INCLUDES = \ +-I. \ +-I$(ROOT_DIR)/mahony \ +-I$(ROOT_DIR)/math_sdr \ +-I$(ROOT_DIR)/test/framework/src + +C_DEFS = \ +-DUNIT_TEST + +CFLAGS = $(C_INCLUDES) $(C_DEFS) $(OPT) -Wall -g + +CFLAGS += -Wno-unused-function +CFLAGS += -Wno-unused-variable +CFLAGS += -ftest-coverage +CFLAGS += -fprofile-arcs +ifeq ($(DEBUG), 1) +CFLAGS += -DDEBUG +else +CFLAGS += -DRELBLD +endif + +################################################################ +# build +################################################################ +all: clean $(BUILD_DIR)/$(TARGET) + +OBJECTS = $(addprefix $(BUILD_DIR)/,$(notdir $(C_SOURCES:.c=.o))) +vpath %.c $(sort $(dir $(C_SOURCES))) + +$(BUILD_DIR)/%.o: %.c $(BUILD_DIR) + $(CC) -c $(CFLAGS) $< -o $@ + +$(BUILD_DIR)/$(TARGET): $(OBJECTS) + $(CC) $(OBJECTS) -o $@ -lgcov -lm + +$(BUILD_DIR): + mkdir $@ + +################################################################ +# test +################################################################ +test: + @echo THIS TEST MUST BE EXECUTED IN A BASH TERMINAL. CMD/PS do not work. + -rm -fR $(BUILD_DIR) + $(MAKE) all + $(BUILD_DIR)/$(TARGET) + tail -n 7 "results.txt" + mkdir -p coverage + gcovr $(BUILD_DIR) \ + --filter "$(ROOT_DIR)/mahony/mahony.c" \ + --filter "$(ROOT_DIR)/math_sdr/math_sdr.c" \ + --html-details coverage/coverage.html \ + --json coverage/coverage.json + +################################################################ +# clean +################################################################ +clean: + -rm -fR $(BUILD_DIR) diff --git a/test/mahony/main.h b/test/mahony/main.h new file mode 100644 index 0000000..55e71c4 --- /dev/null +++ b/test/mahony/main.h @@ -0,0 +1,8 @@ +#ifndef TEST_MAHONY_MAIN_H +#define TEST_MAHONY_MAIN_H + +#include +#include +#include + +#endif /* TEST_MAHONY_MAIN_H */ \ No newline at end of file diff --git a/test/mahony/test_mahony.c b/test/mahony/test_mahony.c new file mode 100644 index 0000000..10f9bde --- /dev/null +++ b/test/mahony/test_mahony.c @@ -0,0 +1,562 @@ +/******************************************************************************* + * + * FILE: + * test_mahony.c + * + * DESCRIPTION: + * Unit tests for the Mahony attitude filter. + * + ******************************************************************************/ + +/*------------------------------------------------------------------------------ + Standard Includes + ------------------------------------------------------------------------------*/ +#include +#include +#include + +#define TEST_PI 3.14159265358979323846f +#define TEST_TOLERANCE 0.001f + +/*------------------------------------------------------------------------------ + Project Includes + ------------------------------------------------------------------------------*/ +#include "mahony.h" +#include "sdrtf_pub.h" + +/*------------------------------------------------------------------------------ + Test Helpers + ------------------------------------------------------------------------------*/ + +static void assert_quat_components + ( + const char *description, + QUAT actual, + QUAT expected + ) +{ +TEST_begin_nested_case(description); + +TEST_ASSERT_EQ_FLOAT("Quaternion w component", actual.w, expected.w); +TEST_ASSERT_EQ_FLOAT("Quaternion x component", actual.x, expected.x); +TEST_ASSERT_EQ_FLOAT("Quaternion y component", actual.y, expected.y); +TEST_ASSERT_EQ_FLOAT("Quaternion z component", actual.z, expected.z); + +TEST_end_nested_case(); + +} /* assert_quat_components */ + + +/*------------------------------------------------------------------------------ + Initialization Tests + ------------------------------------------------------------------------------*/ + +void test_mahony_init_identity_attitude + ( + void + ) +{ +MAHONY_FILTER filter; + +QUAT identity = + { + .w = 1.0f, + .x = 0.0f, + .y = 0.0f, + .z = 0.0f + }; + +TEST_ASSERT_TRUE + ( + "Mahony initialization succeeds", + mahony_init + ( + &filter, + identity, + 1.0f, + 0.0f + ) + ); + +assert_quat_components + ( + "Identity attitude remains identity", + filter.attitude, + identity + ); + +} /* test_mahony_init_identity_attitude */ + + +void test_mahony_init_normalizes_attitude + ( + void + ) +{ +MAHONY_FILTER filter; + +QUAT initial_attitude = + { + .w = 2.0f, + .x = 0.0f, + .y = 0.0f, + .z = 0.0f + }; + +QUAT expected = + { + .w = 1.0f, + .x = 0.0f, + .y = 0.0f, + .z = 0.0f + }; + +TEST_ASSERT_TRUE + ( + "Mahony initialization succeeds", + mahony_init + ( + &filter, + initial_attitude, + 1.0f, + 0.0f + ) + ); + +assert_quat_components + ( + "Initial attitude is normalized", + filter.attitude, + expected + ); + +} /* test_mahony_init_normalizes_attitude */ + + +void test_mahony_init_zero_quaternion_uses_identity + ( + void + ) +{ +MAHONY_FILTER filter; + +QUAT zero_quaternion = + { + .w = 0.0f, + .x = 0.0f, + .y = 0.0f, + .z = 0.0f + }; + +QUAT expected = + { + .w = 1.0f, + .x = 0.0f, + .y = 0.0f, + .z = 0.0f + }; + +TEST_ASSERT_TRUE + ( + "Mahony initialization succeeds", + mahony_init + ( + &filter, + zero_quaternion, + 1.0f, + 0.0f + ) + ); + +assert_quat_components + ( + "Zero quaternion falls back to identity", + filter.attitude, + expected + ); + +} /* test_mahony_init_zero_quaternion_uses_identity */ + + +void test_mahony_init_clears_integral_error + ( + void + ) +{ +MAHONY_FILTER filter; + +QUAT identity = + { + .w = 1.0f, + .x = 0.0f, + .y = 0.0f, + .z = 0.0f + }; + +TEST_ASSERT_TRUE + ( + "Mahony initialization succeeds", + mahony_init + ( + &filter, + identity, + 1.0f, + 0.1f + ) + ); + +TEST_ASSERT_EQ_FLOAT + ( + "Integral error x initializes to zero", + filter.integral_error.x, + 0.0f + ); + +TEST_ASSERT_EQ_FLOAT + ( + "Integral error y initializes to zero", + filter.integral_error.y, + 0.0f + ); + +TEST_ASSERT_EQ_FLOAT + ( + "Integral error z initializes to zero", + filter.integral_error.z, + 0.0f + ); + +} /* test_mahony_init_clears_integral_error */ + + +void test_mahony_init_rejects_null_filter + ( + void + ) +{ +QUAT identity = + { + .w = 1.0f, + .x = 0.0f, + .y = 0.0f, + .z = 0.0f + }; + +TEST_ASSERT_FALSE + ( + "Null filter is rejected", + mahony_init + ( + NULL, + identity, + 1.0f, + 0.0f + ) + ); + +} /* test_mahony_init_rejects_null_filter */ + + +void test_mahony_init_rejects_negative_gain + ( + void + ) +{ +MAHONY_FILTER filter; + +QUAT identity = + { + .w = 1.0f, + .x = 0.0f, + .y = 0.0f, + .z = 0.0f + }; + +TEST_ASSERT_FALSE + ( + "Negative proportional gain is rejected", + mahony_init + ( + &filter, + identity, + -1.0f, + 0.0f + ) + ); + +TEST_ASSERT_FALSE + ( + "Negative integral gain is rejected", + mahony_init + ( + &filter, + identity, + 1.0f, + -0.1f + ) + ); + +} /* test_mahony_init_rejects_negative_gain */ + + +/*------------------------------------------------------------------------------ + Gyroscope Propagation Tests + ------------------------------------------------------------------------------*/ + +void test_mahony_update_gyro_zero_rate + ( + void + ) +{ +MAHONY_FILTER filter; + +QUAT identity = + { + .w = 1.0f, + .x = 0.0f, + .y = 0.0f, + .z = 0.0f + }; + +VECTOR_3F zero_rate = + { + .x = 0.0f, + .y = 0.0f, + .z = 0.0f + }; + +TEST_ASSERT_TRUE + ( + "Mahony initialization succeeds", + mahony_init + ( + &filter, + identity, + 0.0f, + 0.0f + ) + ); + +TEST_ASSERT_TRUE + ( + "Zero-rate update succeeds", + mahony_update_gyro + ( + &filter, + zero_rate, + 0.01f + ) + ); + +assert_quat_components + ( + "Zero angular velocity preserves identity", + filter.attitude, + identity + ); + +} /* test_mahony_update_gyro_zero_rate */ + + +void test_mahony_update_gyro_positive_yaw + ( + void + ) +{ +int32_t index; + +MAHONY_FILTER filter; + +QUAT identity = + { + .w = 1.0f, + .x = 0.0f, + .y = 0.0f, + .z = 0.0f + }; + +VECTOR_3F gyro_body_rad_s = + { + .x = 0.0f, + .y = 0.0f, + .z = 0.5f * TEST_PI + }; + +QUAT body_x = + { + .w = 0.0f, + .x = 1.0f, + .y = 0.0f, + .z = 0.0f + }; + +TEST_ASSERT_TRUE + ( + "Mahony initialization succeeds", + mahony_init + ( + &filter, + identity, + 0.0f, + 0.0f + ) + ); + +for ( index = 0; index < 1000; index++ ) + { + TEST_ASSERT_TRUE + ( + "Positive-yaw propagation succeeds", + mahony_update_gyro + ( + &filter, + gyro_body_rad_s, + 0.001f + ) + ); + } + +TEST_ASSERT_EQ_FLOAT + ( + "Positive 90-degree yaw quaternion w component", + filter.attitude.w, + cosf(0.25f * TEST_PI) + ); + +TEST_ASSERT_EQ_FLOAT + ( + "Positive 90-degree yaw quaternion z component", + filter.attitude.z, + sinf(0.25f * TEST_PI) + ); + +QUAT world_vector = quat_rotate_body_to_world + ( + filter.attitude, + body_x + ); + +TEST_ASSERT_EQ_FLOAT + ( + "Positive yaw rotates body positive X to world positive Y", + world_vector.y, + 1.0f + ); + +} /* test_mahony_update_gyro_positive_yaw */ + + +void test_mahony_update_gyro_rejects_invalid_delta_time + ( + void + ) +{ +MAHONY_FILTER filter; + +QUAT identity = + { + .w = 1.0f, + .x = 0.0f, + .y = 0.0f, + .z = 0.0f + }; + +VECTOR_3F zero_rate = + { + .x = 0.0f, + .y = 0.0f, + .z = 0.0f + }; + +TEST_ASSERT_TRUE + ( + "Mahony initialization succeeds", + mahony_init + ( + &filter, + identity, + 0.0f, + 0.0f + ) + ); + +TEST_ASSERT_FALSE + ( + "Zero delta time is rejected", + mahony_update_gyro + ( + &filter, + zero_rate, + 0.0f + ) + ); + +TEST_ASSERT_FALSE + ( + "Negative delta time is rejected", + mahony_update_gyro + ( + &filter, + zero_rate, + -0.01f + ) + ); + +} /* test_mahony_update_gyro_rejects_invalid_delta_time */ + +/*------------------------------------------------------------------------------ + Main + ------------------------------------------------------------------------------*/ + +int main + ( + void + ) +{ +unit_test tests[] = + { + { + "mahony_init_identity_attitude", + test_mahony_init_identity_attitude + }, + { + "mahony_init_normalizes_attitude", + test_mahony_init_normalizes_attitude + }, + { + "mahony_init_zero_quaternion_uses_identity", + test_mahony_init_zero_quaternion_uses_identity + }, + { + "mahony_init_clears_integral_error", + test_mahony_init_clears_integral_error + }, + { + "mahony_init_rejects_null_filter", + test_mahony_init_rejects_null_filter + }, + { + "mahony_init_rejects_negative_gain", + test_mahony_init_rejects_negative_gain + }, + { + "mahony_update_gyro_zero_rate", + test_mahony_update_gyro_zero_rate + }, + { + "mahony_update_gyro_positive_yaw", + test_mahony_update_gyro_positive_yaw + }, + { + "mahony_update_gyro_rejects_invalid_delta_time", + test_mahony_update_gyro_rejects_invalid_delta_time + } + }; + +TEST_INITIALIZE_TEST("mahony.c", tests); + +} /* main */ + +/******************************************************************************* + * END OF FILE + ******************************************************************************/ \ No newline at end of file From 41be529b540ebb22a6e81452d9f1ac3673289742 Mon Sep 17 00:00:00 2001 From: Bjorn Date: Fri, 24 Jul 2026 17:09:15 -0700 Subject: [PATCH 2/7] Add proportional accelerometer correction to Mahony filter --- mahony/mahony.c | 201 +++++++++++ mahony/mahony.h | 25 ++ test/mahony/test_mahony.c | 735 +++++++++++++++++++++++++++++++++++++- 3 files changed, 953 insertions(+), 8 deletions(-) diff --git a/mahony/mahony.c b/mahony/mahony.c index cc6c9b3..823536e 100644 --- a/mahony/mahony.c +++ b/mahony/mahony.c @@ -59,6 +59,104 @@ return } /* mahony_vector_is_finite */ +static float vector_magnitude + ( + VECTOR_3F vector + ) +{ +return sqrtf + ( + vector.x * vector.x + + vector.y * vector.y + + vector.z * vector.z + ); + +} /* vector_magnitude */ + + +static bool vector_normalize + ( + VECTOR_3F *vector + ) +{ +float magnitude; + +if ( vector == NULL ) + { + return false; + } + +if ( !mahony_vector_is_finite(*vector) ) + { + return false; + } + +magnitude = vector_magnitude(*vector); + +if ( magnitude <= 0.0f || !isfinite(magnitude) ) + { + return false; + } + +vector->x /= magnitude; +vector->y /= magnitude; +vector->z /= magnitude; + +return true; + +} /* vector_normalize */ + + +static VECTOR_3F vector_cross + ( + VECTOR_3F a, + VECTOR_3F b + ) +{ +VECTOR_3F result; + +result.x = a.y * b.z - a.z * b.y; +result.y = a.z * b.x - a.x * b.z; +result.z = a.x * b.y - a.y * b.x; + +return result; + +} /* vector_cross */ + + +static VECTOR_3F vector_add + ( + VECTOR_3F a, + VECTOR_3F b + ) +{ +VECTOR_3F result; + +result.x = a.x + b.x; +result.y = a.y + b.y; +result.z = a.z + b.z; + +return result; + +} /* vector_add */ + + +static VECTOR_3F vector_scale + ( + VECTOR_3F vector, + float scalar + ) +{ +VECTOR_3F result; + +result.x = vector.x * scalar; +result.y = vector.y * scalar; +result.z = vector.z * scalar; + +return result; + +} /* vector_scale */ + /*------------------------------------------------------------------------------ Public Functions @@ -107,6 +205,13 @@ return true; } /* mahony_init */ +/* + * Verify the filter, gyro, and timestep are valid. + * Convert the body-frame angular velocity into a pure quaternion. + * Use the quaternion differential equation to calculate how quickly the + * body-to-world attitude is changing. Multiply that derivative by the + * timestep, add it to the current attitude, and normalize the result. + */ bool mahony_update_gyro ( @@ -181,6 +286,102 @@ return true; } /* mahony_update_gyro */ +bool mahony_update_imu + ( + MAHONY_FILTER *filter, + VECTOR_3F gyro_body_rad_s, + VECTOR_3F accel_body, + float delta_time_s, + bool use_accel + ) +{ +QUAT gravity_world; +QUAT gravity_body_quat; + +VECTOR_3F gravity_estimated_body; +VECTOR_3F attitude_error; +VECTOR_3F proportional_correction; +VECTOR_3F gyro_corrected; + +if ( filter == NULL ) + { + return false; + } + +if ( !mahony_vector_is_finite(gyro_body_rad_s) ) + { + return false; + } + +if ( !isfinite(delta_time_s) || + delta_time_s <= 0.0f ) + { + return false; + } + +gyro_corrected = gyro_body_rad_s; + +/* + * Accelerometer feedback is optional. If the measurement is invalid or has + * zero magnitude, continue with gyroscope-only propagation. + */ +if ( use_accel ) + { + if ( vector_normalize(&accel_body) ) + { + /* + * The attitude quaternion represents the body-to-world rotation. + * Rotate the fixed world gravity direction into the body frame to + * predict where gravity should appear according to the estimate. + */ + gravity_world.w = 0.0f; + gravity_world.x = 0.0f; + gravity_world.y = 0.0f; + gravity_world.z = 1.0f; + + gravity_body_quat = quat_rotate_world_to_body + ( + filter->attitude, + gravity_world + ); + + gravity_estimated_body.x = gravity_body_quat.x; + gravity_estimated_body.y = gravity_body_quat.y; + gravity_estimated_body.z = gravity_body_quat.z; + + /* + * measured x estimated produces a correction that drives the + * estimated gravity direction toward the measured direction. + */ + attitude_error = vector_cross + ( + accel_body, + gravity_estimated_body + ); + + proportional_correction = vector_scale + ( + attitude_error, + filter->proportional_gain + ); + + gyro_corrected = vector_add + ( + gyro_corrected, + proportional_correction + ); + } + } + +return mahony_update_gyro + ( + filter, + gyro_corrected, + delta_time_s + ); + +} /* mahony_update_imu */ + /******************************************************************************* * END OF FILE ******************************************************************************/ \ No newline at end of file diff --git a/mahony/mahony.h b/mahony/mahony.h index d56cac5..311e2a1 100644 --- a/mahony/mahony.h +++ b/mahony/mahony.h @@ -101,6 +101,31 @@ bool mahony_update_gyro float delta_time_s ); +/** + * @brief Updates attitude using gyroscope propagation and accelerometer + * proportional feedback. + * + * The gyroscope must be expressed in the body frame in radians per second. + * The accelerometer must be expressed in the body frame. Its magnitude is + * removed internally because the filter uses only its measured direction. + * + * @param filter Initialized filter instance. + * @param gyro_body_rad_s Body-frame angular velocity in radians per second. + * @param accel_body Body-frame accelerometer measurement. + * @param delta_time_s Elapsed time in seconds. + * @param use_accel Whether accelerometer feedback should be applied. + * + * @return true when the attitude was updated; otherwise false. + */ +bool mahony_update_imu + ( + MAHONY_FILTER *filter, + VECTOR_3F gyro_body_rad_s, + VECTOR_3F accel_body, + float delta_time_s, + bool use_accel + ); + #ifdef __cplusplus } #endif diff --git a/test/mahony/test_mahony.c b/test/mahony/test_mahony.c index 10f9bde..5d0bd61 100644 --- a/test/mahony/test_mahony.c +++ b/test/mahony/test_mahony.c @@ -51,6 +51,16 @@ TEST_end_nested_case(); Initialization Tests ------------------------------------------------------------------------------*/ + /** + * @brief Verifies that the filter accepts an identity initial attitude. + * + * This test initializes the filter with a unit identity quaternion and checks + * that initialization succeeds without changing the attitude. + * + * This is important because identity represents the simplest valid attitude + * and is the expected starting state when the body and world frames are + * aligned. + */ void test_mahony_init_identity_attitude ( void @@ -87,7 +97,16 @@ assert_quat_components } /* test_mahony_init_identity_attitude */ - +/** + * @brief Verifies that the initial attitude quaternion is normalized. + * + * This test initializes the filter with a valid but non-unit quaternion and + * checks that the stored attitude is converted to unit length. + * + * This is important because only unit quaternions represent pure rotations. + * Quaternion propagation and vector rotation can produce invalid results if + * the attitude quaternion is not normalized. + */ void test_mahony_init_normalizes_attitude ( void @@ -132,7 +151,16 @@ assert_quat_components } /* test_mahony_init_normalizes_attitude */ - +/** + * @brief Verifies that a zero quaternion falls back to identity. + * + * This test initializes the filter with a quaternion whose components are all + * zero and checks that quaternion normalization produces the identity + * attitude. + * + * This is important because a zero quaternion does not represent a valid + * rotation and cannot be normalized through ordinary division. + */ void test_mahony_init_zero_quaternion_uses_identity ( void @@ -177,7 +205,15 @@ assert_quat_components } /* test_mahony_init_zero_quaternion_uses_identity */ - +/** + * @brief Verifies that the integral correction state starts at zero. + * + * This test initializes the filter and checks that all components of the + * accumulated integral error are cleared. + * + * This is important because stale integral error would introduce a false gyro + * correction as soon as the filter begins operating. + */ void test_mahony_init_clears_integral_error ( void @@ -228,7 +264,15 @@ TEST_ASSERT_EQ_FLOAT } /* test_mahony_init_clears_integral_error */ - +/** + * @brief Verifies that initialization rejects a null filter pointer. + * + * This test calls mahony_init() without a valid filter instance and checks + * that the function reports failure. + * + * This is important because dereferencing a null filter pointer would cause + * undefined behavior or a runtime fault on the flight computer. + */ void test_mahony_init_rejects_null_filter ( void @@ -256,7 +300,16 @@ TEST_ASSERT_FALSE } /* test_mahony_init_rejects_null_filter */ - +/** + * @brief Verifies that negative feedback gains are rejected. + * + * This test attempts to initialize the filter with negative proportional and + * integral gains and checks that initialization fails. + * + * This is important because negative gains would reverse the intended + * feedback direction and could cause attitude errors to grow rather than + * converge. + */ void test_mahony_init_rejects_negative_gain ( void @@ -303,6 +356,15 @@ TEST_ASSERT_FALSE Gyroscope Propagation Tests ------------------------------------------------------------------------------*/ + /** + * @brief Verifies that zero angular velocity preserves the current attitude. + * + * This test begins at the identity attitude, supplies a zero gyroscope vector, + * and checks that gyro propagation does not rotate the attitude. + * + * This is important because a stationary body should not develop artificial + * rotation when the measured angular rate is zero. + */ void test_mahony_update_gyro_zero_rate ( void @@ -357,7 +419,19 @@ assert_quat_components } /* test_mahony_update_gyro_zero_rate */ - +/** + * @brief Verifies positive yaw propagation from body-frame angular velocity. + * + * This test applies a positive 90-degree-per-second body Z-axis angular rate + * for one second and checks that the resulting quaternion represents an + * approximately positive 90-degree yaw. + * + * It also rotates the body positive X axis into the world frame to verify that + * it points toward world positive Y. + * + * This is important because it confirms the quaternion multiplication order, + * rotation sign, gyro units, and body-to-world attitude convention. + */ void test_mahony_update_gyro_positive_yaw ( void @@ -445,7 +519,16 @@ TEST_ASSERT_EQ_FLOAT } /* test_mahony_update_gyro_positive_yaw */ - +/** + * @brief Verifies that gyro propagation rejects invalid timesteps. + * + * This test supplies zero and negative elapsed times and checks that the + * update reports failure. + * + * This is important because attitude propagation requires a positive elapsed + * time. Invalid timing values could reverse propagation or hide timing errors + * in the sensor update path. + */ void test_mahony_update_gyro_rejects_invalid_delta_time ( void @@ -504,6 +587,618 @@ TEST_ASSERT_FALSE } /* test_mahony_update_gyro_rejects_invalid_delta_time */ +/** + * @brief Verifies that aligned gravity produces no attitude correction. + * + * This test begins with an identity attitude, zero angular velocity, and an + * accelerometer measurement aligned with the expected body-frame gravity + * direction. + * + * The measured and estimated gravity vectors should have a zero cross product, + * so the attitude should remain unchanged. + * + * This is important because a correctly aligned filter must not introduce + * artificial rotation when there is no attitude error. + */ +void test_mahony_update_imu_aligned_gravity + ( + void + ) +{ +int32_t index; + +MAHONY_FILTER filter; + +QUAT identity = + { + .w = 1.0f, + .x = 0.0f, + .y = 0.0f, + .z = 0.0f + }; + +VECTOR_3F zero_gyro = + { + .x = 0.0f, + .y = 0.0f, + .z = 0.0f + }; + +VECTOR_3F accel_body = + { + .x = 0.0f, + .y = 0.0f, + .z = GRAVITY + }; + +TEST_ASSERT_TRUE + ( + "Mahony initialization succeeds", + mahony_init + ( + &filter, + identity, + 1.0f, + 0.0f + ) + ); + +for ( index = 0; index < 1000; index++ ) + { + TEST_ASSERT_TRUE + ( + "Aligned IMU update succeeds", + mahony_update_imu + ( + &filter, + zero_gyro, + accel_body, + 0.001f, + true + ) + ); + } + +assert_quat_components + ( + "Aligned gravity preserves identity attitude", + filter.attitude, + identity + ); + +} /* test_mahony_update_imu_aligned_gravity */ + +/** + * @brief Verifies that accelerometer feedback reduces a roll error. + * + * This test initializes the attitude with a positive roll offset while + * supplying zero angular velocity and a stationary gravity measurement. + * Repeated Mahony updates should move the estimated body Z axis closer to the + * world Z axis. + * + * This is important because it verifies that proportional accelerometer + * feedback corrects roll drift and that the cross-product sign is consistent + * with the body-to-world quaternion convention. + */ +void test_mahony_update_imu_roll_error_converges + ( + void + ) +{ +int32_t index; + +MAHONY_FILTER filter; + +const float initial_roll_rad = deg_to_rad(10.0f); + +QUAT initial_attitude = eul_to_quat + ( + 0.0f, + 0.0f, + initial_roll_rad + ); + +VECTOR_3F zero_gyro = + { + .x = 0.0f, + .y = 0.0f, + .z = 0.0f + }; + +VECTOR_3F accel_body = + { + .x = 0.0f, + .y = 0.0f, + .z = GRAVITY + }; + +QUAT body_z = + { + .w = 0.0f, + .x = 0.0f, + .y = 0.0f, + .z = 1.0f + }; + +QUAT initial_world_z = quat_rotate_body_to_world + ( + initial_attitude, + body_z + ); + +TEST_ASSERT_TRUE + ( + "Mahony initialization succeeds", + mahony_init + ( + &filter, + initial_attitude, + 1.0f, + 0.0f + ) + ); + +for ( index = 0; index < 2000; index++ ) + { + TEST_ASSERT_TRUE + ( + "Roll-error correction update succeeds", + mahony_update_imu + ( + &filter, + zero_gyro, + accel_body, + 0.001f, + true + ) + ); + } + +QUAT corrected_world_z = quat_rotate_body_to_world + ( + filter.attitude, + body_z + ); + +TEST_ASSERT_TRUE + ( + "Roll error decreases toward level", + fabsf(corrected_world_z.y) < fabsf(initial_world_z.y) + ); + +TEST_ASSERT_TRUE + ( + "Corrected body Z approaches world Z", + corrected_world_z.z > initial_world_z.z + ); + +} /* test_mahony_update_imu_roll_error_converges */ + +/** + * @brief Verifies that accelerometer feedback reduces a pitch error. + * + * This test initializes the attitude with a positive pitch offset while + * supplying zero angular velocity and a stationary gravity measurement. + * Repeated Mahony updates should move the estimated body Z axis closer to the + * world Z axis. + * + * This is important because it confirms that accelerometer correction works + * on both observable tilt axes rather than only for roll. + */ +void test_mahony_update_imu_pitch_error_converges + ( + void + ) +{ +int32_t index; + +MAHONY_FILTER filter; + +const float initial_pitch_rad = deg_to_rad(10.0f); + +QUAT initial_attitude = eul_to_quat + ( + 0.0f, + initial_pitch_rad, + 0.0f + ); + +VECTOR_3F zero_gyro = + { + .x = 0.0f, + .y = 0.0f, + .z = 0.0f + }; + +VECTOR_3F accel_body = + { + .x = 0.0f, + .y = 0.0f, + .z = GRAVITY + }; + +QUAT body_z = + { + .w = 0.0f, + .x = 0.0f, + .y = 0.0f, + .z = 1.0f + }; + +QUAT initial_world_z = quat_rotate_body_to_world + ( + initial_attitude, + body_z + ); + +TEST_ASSERT_TRUE + ( + "Mahony initialization succeeds", + mahony_init + ( + &filter, + initial_attitude, + 1.0f, + 0.0f + ) + ); + +for ( index = 0; index < 2000; index++ ) + { + TEST_ASSERT_TRUE + ( + "Pitch-error correction update succeeds", + mahony_update_imu + ( + &filter, + zero_gyro, + accel_body, + 0.001f, + true + ) + ); + } + +QUAT corrected_world_z = quat_rotate_body_to_world + ( + filter.attitude, + body_z + ); + +TEST_ASSERT_TRUE + ( + "Pitch error decreases toward level", + fabsf(corrected_world_z.x) < fabsf(initial_world_z.x) + ); + +TEST_ASSERT_TRUE + ( + "Corrected body Z approaches world Z", + corrected_world_z.z > initial_world_z.z + ); + +} /* test_mahony_update_imu_pitch_error_converges */ + +/** + * @brief Verifies that accelerometer feedback does not correct yaw. + * + * This test initializes the attitude with a yaw offset while roll and pitch + * remain level. Because yaw rotation does not change the gravity direction, + * repeated accelerometer corrections should leave the yaw attitude unchanged. + * + * This is important because gravity provides roll and pitch information but + * contains no heading information. Yaw correction requires another reference, + * such as a calibrated magnetometer. + */ +void test_mahony_update_imu_yaw_error_does_not_converge + ( + void + ) +{ +int32_t index; + +MAHONY_FILTER filter; + +const float initial_yaw_rad = deg_to_rad(20.0f); + +QUAT initial_attitude = eul_to_quat + ( + initial_yaw_rad, + 0.0f, + 0.0f + ); + +VECTOR_3F zero_gyro = + { + .x = 0.0f, + .y = 0.0f, + .z = 0.0f + }; + +VECTOR_3F accel_body = + { + .x = 0.0f, + .y = 0.0f, + .z = GRAVITY + }; + +QUAT body_x = + { + .w = 0.0f, + .x = 1.0f, + .y = 0.0f, + .z = 0.0f + }; + +QUAT initial_world_x = quat_rotate_body_to_world + ( + initial_attitude, + body_x + ); + +TEST_ASSERT_TRUE + ( + "Mahony initialization succeeds", + mahony_init + ( + &filter, + initial_attitude, + 1.0f, + 0.0f + ) + ); + +for ( index = 0; index < 2000; index++ ) + { + TEST_ASSERT_TRUE + ( + "Yaw-only correction update succeeds", + mahony_update_imu + ( + &filter, + zero_gyro, + accel_body, + 0.001f, + true + ) + ); + } + +QUAT final_world_x = quat_rotate_body_to_world + ( + filter.attitude, + body_x + ); + +TEST_ASSERT_EQ_FLOAT + ( + "Accelerometer does not correct yaw X component", + final_world_x.x, + initial_world_x.x + ); + +TEST_ASSERT_EQ_FLOAT + ( + "Accelerometer does not correct yaw Y component", + final_world_x.y, + initial_world_x.y + ); + +} /* test_mahony_update_imu_yaw_error_does_not_converge */ + +/** + * @brief Verifies that a zero accelerometer vector falls back to gyro-only + * propagation. + * + * This test runs two identical filters. One uses mahony_update_gyro(), while + * the other uses mahony_update_imu() with accelerometer feedback requested but + * a zero accelerometer vector. + * + * Both filters should produce the same attitude because the zero vector cannot + * be normalized and must therefore be excluded from feedback. + * + * This is important because an invalid accelerometer sample should not stop + * attitude propagation or introduce undefined normalization behavior. + */ +void test_mahony_update_imu_zero_accel_uses_gyro_only + ( + void + ) +{ +int32_t index; + +MAHONY_FILTER gyro_filter; +MAHONY_FILTER imu_filter; + +QUAT identity = + { + .w = 1.0f, + .x = 0.0f, + .y = 0.0f, + .z = 0.0f + }; + +VECTOR_3F gyro_body_rad_s = + { + .x = 0.0f, + .y = 0.0f, + .z = deg_to_rad(45.0f) + }; + +VECTOR_3F zero_accel = + { + .x = 0.0f, + .y = 0.0f, + .z = 0.0f + }; + +TEST_ASSERT_TRUE + ( + "Gyro filter initialization succeeds", + mahony_init + ( + &gyro_filter, + identity, + 1.0f, + 0.0f + ) + ); + +TEST_ASSERT_TRUE + ( + "IMU filter initialization succeeds", + mahony_init + ( + &imu_filter, + identity, + 1.0f, + 0.0f + ) + ); + +for ( index = 0; index < 1000; index++ ) + { + TEST_ASSERT_TRUE + ( + "Gyro-only update succeeds", + mahony_update_gyro + ( + &gyro_filter, + gyro_body_rad_s, + 0.001f + ) + ); + + TEST_ASSERT_TRUE + ( + "Zero-accelerometer IMU update succeeds", + mahony_update_imu + ( + &imu_filter, + gyro_body_rad_s, + zero_accel, + 0.001f, + true + ) + ); + } + +assert_quat_components + ( + "Zero accelerometer matches gyro-only propagation", + imu_filter.attitude, + gyro_filter.attitude + ); + +} /* test_mahony_update_imu_zero_accel_uses_gyro_only */ + +/** + * @brief Verifies that disabling accelerometer feedback matches gyro-only + * propagation. + * + * This test runs two identical filters while providing a deliberately + * misaligned accelerometer measurement. One filter uses gyro-only propagation, + * and the other calls mahony_update_imu() with use_accel set to false. + * + * Both filters should produce the same attitude because the accelerometer + * measurement must be completely ignored. + * + * This is important because flight-state logic must be able to disable + * accelerometer correction during thrust, vibration, saturation, or other + * high-dynamic conditions. + */ +void test_mahony_update_imu_disabled_accel_uses_gyro_only + ( + void + ) +{ +int32_t index; + +MAHONY_FILTER gyro_filter; +MAHONY_FILTER imu_filter; + +QUAT identity = + { + .w = 1.0f, + .x = 0.0f, + .y = 0.0f, + .z = 0.0f + }; + +VECTOR_3F gyro_body_rad_s = + { + .x = deg_to_rad(30.0f), + .y = 0.0f, + .z = 0.0f + }; + +VECTOR_3F misaligned_accel = + { + .x = GRAVITY, + .y = 0.0f, + .z = 0.0f + }; + +TEST_ASSERT_TRUE + ( + "Gyro filter initialization succeeds", + mahony_init + ( + &gyro_filter, + identity, + 1.0f, + 0.0f + ) + ); + +TEST_ASSERT_TRUE + ( + "IMU filter initialization succeeds", + mahony_init + ( + &imu_filter, + identity, + 1.0f, + 0.0f + ) + ); + +for ( index = 0; index < 1000; index++ ) + { + TEST_ASSERT_TRUE + ( + "Gyro-only update succeeds", + mahony_update_gyro + ( + &gyro_filter, + gyro_body_rad_s, + 0.001f + ) + ); + + TEST_ASSERT_TRUE + ( + "Disabled-accelerometer IMU update succeeds", + mahony_update_imu + ( + &imu_filter, + gyro_body_rad_s, + misaligned_accel, + 0.001f, + false + ) + ); + } + +assert_quat_components + ( + "Disabled accelerometer matches gyro-only propagation", + imu_filter.attitude, + gyro_filter.attitude + ); + +} /* test_mahony_update_imu_disabled_accel_uses_gyro_only */ + /*------------------------------------------------------------------------------ Main ------------------------------------------------------------------------------*/ @@ -550,7 +1245,31 @@ unit_test tests[] = { "mahony_update_gyro_rejects_invalid_delta_time", test_mahony_update_gyro_rejects_invalid_delta_time - } + }, + { + "mahony_update_imu_aligned_gravity", + test_mahony_update_imu_aligned_gravity + }, + { + "mahony_update_imu_roll_error_converges", + test_mahony_update_imu_roll_error_converges + }, + { + "mahony_update_imu_pitch_error_converges", + test_mahony_update_imu_pitch_error_converges + }, + { + "mahony_update_imu_yaw_error_does_not_converge", + test_mahony_update_imu_yaw_error_does_not_converge + }, + { + "mahony_update_imu_zero_accel_uses_gyro_only", + test_mahony_update_imu_zero_accel_uses_gyro_only + }, + { + "mahony_update_imu_disabled_accel_uses_gyro_only", + test_mahony_update_imu_disabled_accel_uses_gyro_only + }, }; TEST_INITIALIZE_TEST("mahony.c", tests); From c21ff3437d7bac04a401000a37409a2daeb74b6e Mon Sep 17 00:00:00 2001 From: Bjorn Date: Sat, 25 Jul 2026 08:13:43 -0700 Subject: [PATCH 3/7] Add Mahony attitude diagnostic output tests --- test/mahony/test_mahony.c | 416 ++++++++++++++++++++++++++++++++++++++ 1 file changed, 416 insertions(+) diff --git a/test/mahony/test_mahony.c b/test/mahony/test_mahony.c index 5d0bd61..847020c 100644 --- a/test/mahony/test_mahony.c +++ b/test/mahony/test_mahony.c @@ -46,6 +46,177 @@ TEST_end_nested_case(); } /* assert_quat_components */ +/** + * @brief Limits a floating-point value to a specified range. + */ +static float clamp_float + ( + float value, + float minimum, + float maximum + ) +{ +if ( value < minimum ) + { + return minimum; + } + +if ( value > maximum ) + { + return maximum; + } + +return value; + +} /* clamp_float */ + + +/** + * @brief Converts a body-to-world quaternion into ZYX Euler angles. + * + * The returned values represent roll about X, pitch about Y, and yaw about Z. + * Angles are returned in degrees for readable diagnostic output. + */ +static VECTOR_3F quat_to_euler_deg + ( + QUAT attitude + ) +{ +VECTOR_3F angles_deg; + +float sin_roll_cos_pitch; +float cos_roll_cos_pitch; +float sin_pitch; +float sin_yaw_cos_pitch; +float cos_yaw_cos_pitch; + +attitude = quat_normalize(attitude); + +sin_roll_cos_pitch = + 2.0f * + ( + attitude.w * attitude.x + + attitude.y * attitude.z + ); + +cos_roll_cos_pitch = + 1.0f - + 2.0f * + ( + attitude.x * attitude.x + + attitude.y * attitude.y + ); + +sin_pitch = + 2.0f * + ( + attitude.w * attitude.y - + attitude.z * attitude.x + ); + +sin_pitch = clamp_float + ( + sin_pitch, + -1.0f, + 1.0f + ); + +sin_yaw_cos_pitch = + 2.0f * + ( + attitude.w * attitude.z + + attitude.x * attitude.y + ); + +cos_yaw_cos_pitch = + 1.0f - + 2.0f * + ( + attitude.y * attitude.y + + attitude.z * attitude.z + ); + +angles_deg.x = rad_to_deg + ( + atan2f + ( + sin_roll_cos_pitch, + cos_roll_cos_pitch + ) + ); + +angles_deg.y = rad_to_deg + ( + asinf(sin_pitch) + ); + +angles_deg.z = rad_to_deg + ( + atan2f + ( + sin_yaw_cos_pitch, + cos_yaw_cos_pitch + ) + ); + +return angles_deg; + +} /* quat_to_euler_deg */ + + +/** + * @brief Prints one attitude sample as Euler angles and quaternion components. + */ +static void print_attitude_sample + ( + int32_t interval, + float time_s, + QUAT attitude + ) +{ +VECTOR_3F angles_deg = quat_to_euler_deg(attitude); + +printf + ( + "%3ld | %6.2f | %9.3f %9.3f %9.3f | " + "%9.5f %9.5f %9.5f %9.5f\n", + (long)interval, + time_s, + angles_deg.x, + angles_deg.y, + angles_deg.z, + attitude.w, + attitude.x, + attitude.y, + attitude.z + ); + +} /* print_attitude_sample */ + + +/** + * @brief Prints the diagnostic attitude-table heading. + */ +static void print_attitude_heading + ( + const char *title + ) +{ +printf("\n%s\n", title); +printf + ( + "Int | Time s | Roll deg Pitch deg Yaw deg | " + " w x y z\n" + ); + +printf + ( + "----+--------+-------------------------------+" + "----------------------------------------\n" + ); + +} /* print_attitude_heading */ + /*------------------------------------------------------------------------------ Initialization Tests @@ -1199,6 +1370,243 @@ assert_quat_components } /* test_mahony_update_imu_disabled_accel_uses_gyro_only */ +/** + * @brief Prints attitude propagation over ten gyro-only update intervals. + * + * This test simulates a rocket rotating simultaneously about all three body + * axes. It prints the estimated roll, pitch, yaw, and body-to-world quaternion + * after each update. + * + * This is useful for visually confirming that angular velocity accumulates + * smoothly, quaternion components change continuously, and normalization keeps + * the quaternion valid throughout propagation. + */ +void test_mahony_print_gyro_propagation + ( + void + ) +{ +int32_t interval; + +const int32_t interval_count = 10; +const float delta_time_s = 0.1f; + +MAHONY_FILTER filter; + +QUAT identity = + { + .w = 1.0f, + .x = 0.0f, + .y = 0.0f, + .z = 0.0f + }; + +/* + * Simulated body-frame rocket rotation: + * + * Roll rate = 10 degrees per second + * Pitch rate = 5 degrees per second + * Yaw rate = 20 degrees per second + */ +VECTOR_3F gyro_body_rad_s = + { + .x = deg_to_rad(10.0f), + .y = deg_to_rad(5.0f), + .z = deg_to_rad(20.0f) + }; + +TEST_ASSERT_TRUE + ( + "Gyro diagnostic filter initialization succeeds", + mahony_init + ( + &filter, + identity, + 0.0f, + 0.0f + ) + ); + +print_attitude_heading + ( + "GYROSCOPE-ONLY ATTITUDE PROPAGATION" + ); + +print_attitude_sample + ( + 0, + 0.0f, + filter.attitude + ); + +for ( interval = 1; interval <= interval_count; interval++ ) + { + TEST_ASSERT_TRUE + ( + "Gyro diagnostic propagation succeeds", + mahony_update_gyro + ( + &filter, + gyro_body_rad_s, + delta_time_s + ) + ); + + print_attitude_sample + ( + interval, + interval * delta_time_s, + filter.attitude + ); + } + +/* + * A propagated attitude must remain a unit quaternion. + */ +float quaternion_norm = sqrtf + ( + filter.attitude.w * filter.attitude.w + + filter.attitude.x * filter.attitude.x + + filter.attitude.y * filter.attitude.y + + filter.attitude.z * filter.attitude.z + ); + +TEST_ASSERT_TRUE + ( + "Gyro-propagated quaternion remains normalized", + fabsf(quaternion_norm - 1.0f) < 0.001f + ); + +} /* test_mahony_print_gyro_propagation */ + +/** + * @brief Prints proportional accelerometer correction over ten intervals. + * + * This test simulates a low-dynamic or coasting flight period. The estimated + * attitude begins with roll and pitch errors, the gyro reports no rotation, + * and the accelerometer supplies a stable gravity direction. + * + * The printed output should show roll and pitch moving toward zero while yaw + * remains approximately unchanged. This is useful for visualizing how Mahony + * proportional feedback corrects observable tilt error without pretending + * that gravity provides heading information. + */ +void test_mahony_print_accelerometer_correction + ( + void + ) +{ +int32_t interval; + +const int32_t interval_count = 10; +const float delta_time_s = 0.1f; + +MAHONY_FILTER filter; + +QUAT initial_attitude = eul_to_quat + ( + deg_to_rad(20.0f), + deg_to_rad(-10.0f), + deg_to_rad(15.0f) + ); + +VECTOR_3F zero_gyro = + { + .x = 0.0f, + .y = 0.0f, + .z = 0.0f + }; + +/* + * This represents a low-dynamic condition where the measured acceleration + * direction is a usable gravity reference. + */ +VECTOR_3F accel_body = + { + .x = 0.0f, + .y = 0.0f, + .z = GRAVITY + }; + +QUAT body_z = + { + .w = 0.0f, + .x = 0.0f, + .y = 0.0f, + .z = 1.0f + }; + +QUAT initial_world_z = quat_rotate_body_to_world + ( + initial_attitude, + body_z + ); + +TEST_ASSERT_TRUE + ( + "Accelerometer diagnostic filter initialization succeeds", + mahony_init + ( + &filter, + initial_attitude, + 1.5f, + 0.0f + ) + ); + +print_attitude_heading + ( + "MAHONY PROPORTIONAL ACCELEROMETER CORRECTION" + ); + +print_attitude_sample + ( + 0, + 0.0f, + filter.attitude + ); + +for ( interval = 1; interval <= interval_count; interval++ ) + { + TEST_ASSERT_TRUE + ( + "Accelerometer diagnostic update succeeds", + mahony_update_imu + ( + &filter, + zero_gyro, + accel_body, + delta_time_s, + true + ) + ); + + print_attitude_sample + ( + interval, + interval * delta_time_s, + filter.attitude + ); + } + +QUAT final_world_z = quat_rotate_body_to_world + ( + filter.attitude, + body_z + ); + +/* + * A level attitude has the body Z axis aligned with world Z. Therefore, its + * world Z component should increase as the tilt error is corrected. + */ +TEST_ASSERT_TRUE + ( + "Accelerometer correction improves body Z alignment", + final_world_z.z > initial_world_z.z + ); + +} /* test_mahony_print_accelerometer_correction */ + /*------------------------------------------------------------------------------ Main ------------------------------------------------------------------------------*/ @@ -1270,6 +1678,14 @@ unit_test tests[] = "mahony_update_imu_disabled_accel_uses_gyro_only", test_mahony_update_imu_disabled_accel_uses_gyro_only }, + { + "mahony_print_gyro_propagation", + test_mahony_print_gyro_propagation + }, + { + "mahony_print_accelerometer_correction", + test_mahony_print_accelerometer_correction + }, }; TEST_INITIALIZE_TEST("mahony.c", tests); From d47061aaefb806d119b3739f2b6c765b508c652e Mon Sep 17 00:00:00 2001 From: Bjorn Date: Sat, 25 Jul 2026 15:42:51 -0700 Subject: [PATCH 4/7] Add accelerometer validity gating to Mahony filter --- mahony/mahony.c | 31 ++- mahony/mahony.h | 5 + test/mahony/test_mahony.c | 412 ++++++++++++++++++++++++++++++++++++++ 3 files changed, 447 insertions(+), 1 deletion(-) diff --git a/mahony/mahony.c b/mahony/mahony.c index 823536e..9adf520 100644 --- a/mahony/mahony.c +++ b/mahony/mahony.c @@ -19,6 +19,18 @@ ------------------------------------------------------------------------------*/ #include "mahony.h" +/*------------------------------------------------------------------------------ + Private Macros + ------------------------------------------------------------------------------*/ + +/* + * Accelerometer feedback is only used when the measured magnitude is reasonably + * close to one g. These are initial software validation thresholds and should + * be tuned later using stationary, vibration, and flight data. + */ +#define MAHONY_ACCEL_MIN_MAGNITUDE (0.85f * GRAVITY) +#define MAHONY_ACCEL_MAX_MAGNITUDE (1.15f * GRAVITY) + /*------------------------------------------------------------------------------ Private Functions ------------------------------------------------------------------------------*/ @@ -303,6 +315,11 @@ VECTOR_3F attitude_error; VECTOR_3F proportional_correction; VECTOR_3F gyro_corrected; +float accel_magnitude; + +bool accel_valid; +bool apply_accel; + if ( filter == NULL ) { return false; @@ -319,13 +336,25 @@ if ( !isfinite(delta_time_s) || return false; } +accel_magnitude = vector_magnitude(accel_body); + +accel_valid = + mahony_vector_is_finite(accel_body) && + isfinite(accel_magnitude) && + accel_magnitude >= MAHONY_ACCEL_MIN_MAGNITUDE && + accel_magnitude <= MAHONY_ACCEL_MAX_MAGNITUDE; + +apply_accel = + use_accel && + accel_valid; + gyro_corrected = gyro_body_rad_s; /* * Accelerometer feedback is optional. If the measurement is invalid or has * zero magnitude, continue with gyroscope-only propagation. */ -if ( use_accel ) +if ( apply_accel ) { if ( vector_normalize(&accel_body) ) { diff --git a/mahony/mahony.h b/mahony/mahony.h index 311e2a1..38e1c48 100644 --- a/mahony/mahony.h +++ b/mahony/mahony.h @@ -109,6 +109,10 @@ bool mahony_update_gyro * The accelerometer must be expressed in the body frame. Its magnitude is * removed internally because the filter uses only its measured direction. * + * Accelerometer feedback is applied only when the caller enables it and the + * measured acceleration magnitude falls within the configured validity range. + * Invalid accelerometer samples are ignored while gyro propagation continues. + * * @param filter Initialized filter instance. * @param gyro_body_rad_s Body-frame angular velocity in radians per second. * @param accel_body Body-frame accelerometer measurement. @@ -117,6 +121,7 @@ bool mahony_update_gyro * * @return true when the attitude was updated; otherwise false. */ + bool mahony_update_imu ( MAHONY_FILTER *filter, diff --git a/test/mahony/test_mahony.c b/test/mahony/test_mahony.c index 847020c..faabae6 100644 --- a/test/mahony/test_mahony.c +++ b/test/mahony/test_mahony.c @@ -1607,6 +1607,402 @@ TEST_ASSERT_TRUE } /* test_mahony_print_accelerometer_correction */ +/** + * @brief Verifies that an accelerometer magnitude below the valid range is + * rejected. + * + * This test compares ordinary gyro propagation against an IMU update using an + * accelerometer vector below the configured minimum magnitude. + * + * This is important because a weak or invalid accelerometer measurement should + * not influence attitude, while gyro propagation must continue normally. + */ +void test_mahony_update_imu_rejects_low_accel_magnitude + ( + void + ) +{ +int32_t index; + +MAHONY_FILTER gyro_filter; +MAHONY_FILTER imu_filter; + +QUAT identity = + { + .w = 1.0f, + .x = 0.0f, + .y = 0.0f, + .z = 0.0f + }; + +VECTOR_3F gyro_body_rad_s = + { + .x = deg_to_rad(20.0f), + .y = 0.0f, + .z = 0.0f + }; + +VECTOR_3F low_accel = + { + .x = 0.0f, + .y = 0.0f, + .z = 0.50f * GRAVITY + }; + +TEST_ASSERT_TRUE + ( + "Gyro filter initialization succeeds", + mahony_init + ( + &gyro_filter, + identity, + 1.0f, + 0.0f + ) + ); + +TEST_ASSERT_TRUE + ( + "IMU filter initialization succeeds", + mahony_init + ( + &imu_filter, + identity, + 1.0f, + 0.0f + ) + ); + +for ( index = 0; index < 1000; index++ ) + { + TEST_ASSERT_TRUE + ( + "Gyro-only update succeeds", + mahony_update_gyro + ( + &gyro_filter, + gyro_body_rad_s, + 0.001f + ) + ); + + TEST_ASSERT_TRUE + ( + "Low-acceleration IMU update succeeds", + mahony_update_imu + ( + &imu_filter, + gyro_body_rad_s, + low_accel, + 0.001f, + true + ) + ); + } + +assert_quat_components + ( + "Low acceleration matches gyro-only propagation", + imu_filter.attitude, + gyro_filter.attitude + ); + +} /* test_mahony_update_imu_rejects_low_accel_magnitude */ + +/** + * @brief Verifies that a non-finite accelerometer sample is rejected. + * + * This test supplies a NaN accelerometer component and compares the result + * against gyro-only propagation. + * + * This is important because corrupted sensor data must not propagate NaN + * values into the attitude quaternion. + */ +void test_mahony_update_imu_rejects_nonfinite_accel + ( + void + ) +{ +MAHONY_FILTER gyro_filter; +MAHONY_FILTER imu_filter; + +QUAT identity = + { + .w = 1.0f, + .x = 0.0f, + .y = 0.0f, + .z = 0.0f + }; + +VECTOR_3F gyro_body_rad_s = + { + .x = 0.0f, + .y = 0.0f, + .z = deg_to_rad(10.0f) + }; + +VECTOR_3F invalid_accel = + { + .x = NAN, + .y = 0.0f, + .z = GRAVITY + }; + +TEST_ASSERT_TRUE + ( + "Gyro filter initialization succeeds", + mahony_init + ( + &gyro_filter, + identity, + 1.0f, + 0.0f + ) + ); + +TEST_ASSERT_TRUE + ( + "IMU filter initialization succeeds", + mahony_init + ( + &imu_filter, + identity, + 1.0f, + 0.0f + ) + ); + +TEST_ASSERT_TRUE + ( + "Gyro-only update succeeds", + mahony_update_gyro + ( + &gyro_filter, + gyro_body_rad_s, + 0.01f + ) + ); + +TEST_ASSERT_TRUE + ( + "Invalid-accelerometer IMU update succeeds", + mahony_update_imu + ( + &imu_filter, + gyro_body_rad_s, + invalid_accel, + 0.01f, + true + ) + ); + +assert_quat_components + ( + "Non-finite acceleration matches gyro-only propagation", + imu_filter.attitude, + gyro_filter.attitude + ); + +} /* test_mahony_update_imu_rejects_nonfinite_accel */ + +/** + * @brief Verifies that a valid one-g accelerometer sample still corrects tilt. + * + * This test starts with a roll error and supplies a valid one-g acceleration + * vector. The corrected body Z axis should move closer to world Z. + * + * This is important because validity gating must reject poor samples without + * blocking legitimate gravity correction. + */ +void test_mahony_update_imu_accepts_valid_accel_magnitude + ( + void + ) +{ +int32_t index; + +MAHONY_FILTER filter; + +QUAT initial_attitude = eul_to_quat + ( + 0.0f, + 0.0f, + deg_to_rad(10.0f) + ); + +VECTOR_3F zero_gyro = + { + .x = 0.0f, + .y = 0.0f, + .z = 0.0f + }; + +VECTOR_3F valid_accel = + { + .x = 0.0f, + .y = 0.0f, + .z = GRAVITY + }; + +QUAT body_z = + { + .w = 0.0f, + .x = 0.0f, + .y = 0.0f, + .z = 1.0f + }; + +QUAT initial_world_z = quat_rotate_body_to_world + ( + initial_attitude, + body_z + ); + +TEST_ASSERT_TRUE + ( + "Mahony initialization succeeds", + mahony_init + ( + &filter, + initial_attitude, + 1.0f, + 0.0f + ) + ); + +for ( index = 0; index < 1000; index++ ) + { + TEST_ASSERT_TRUE + ( + "Valid-acceleration IMU update succeeds", + mahony_update_imu + ( + &filter, + zero_gyro, + valid_accel, + 0.001f, + true + ) + ); + } + +QUAT corrected_world_z = quat_rotate_body_to_world + ( + filter.attitude, + body_z + ); + +TEST_ASSERT_TRUE + ( + "Valid acceleration improves body Z alignment", + corrected_world_z.z > initial_world_z.z + ); + +} /* test_mahony_update_imu_accepts_valid_accel_magnitude */ + +/** + * @brief Verifies that an accelerometer magnitude above the valid range is + * rejected. + * + * This test simulates a high-acceleration condition such as powered ascent. + * The IMU update should ignore the accelerometer and match gyro-only + * propagation. + * + * This is important because thrust acceleration must not be mistaken for the + * gravity direction. + */ +void test_mahony_update_imu_rejects_high_accel_magnitude + ( + void + ) +{ +int32_t index; + +MAHONY_FILTER gyro_filter; +MAHONY_FILTER imu_filter; + +QUAT identity = + { + .w = 1.0f, + .x = 0.0f, + .y = 0.0f, + .z = 0.0f + }; + +VECTOR_3F gyro_body_rad_s = + { + .x = 0.0f, + .y = deg_to_rad(15.0f), + .z = 0.0f + }; + +VECTOR_3F high_accel = + { + .x = 0.0f, + .y = 0.0f, + .z = 2.0f * GRAVITY + }; + +TEST_ASSERT_TRUE + ( + "Gyro filter initialization succeeds", + mahony_init + ( + &gyro_filter, + identity, + 1.0f, + 0.0f + ) + ); + +TEST_ASSERT_TRUE + ( + "IMU filter initialization succeeds", + mahony_init + ( + &imu_filter, + identity, + 1.0f, + 0.0f + ) + ); + +for ( index = 0; index < 1000; index++ ) + { + TEST_ASSERT_TRUE + ( + "Gyro-only update succeeds", + mahony_update_gyro + ( + &gyro_filter, + gyro_body_rad_s, + 0.001f + ) + ); + + TEST_ASSERT_TRUE + ( + "High-acceleration IMU update succeeds", + mahony_update_imu + ( + &imu_filter, + gyro_body_rad_s, + high_accel, + 0.001f, + true + ) + ); + } + +assert_quat_components + ( + "High acceleration matches gyro-only propagation", + imu_filter.attitude, + gyro_filter.attitude + ); + +} /* test_mahony_update_imu_rejects_high_accel_magnitude */ + /*------------------------------------------------------------------------------ Main ------------------------------------------------------------------------------*/ @@ -1686,6 +2082,22 @@ unit_test tests[] = "mahony_print_accelerometer_correction", test_mahony_print_accelerometer_correction }, + { + "mahony_update_imu_rejects_low_accel_magnitude", + test_mahony_update_imu_rejects_low_accel_magnitude + }, + { + "mahony_update_imu_rejects_high_accel_magnitude", + test_mahony_update_imu_rejects_high_accel_magnitude + }, + { + "mahony_update_imu_rejects_nonfinite_accel", + test_mahony_update_imu_rejects_nonfinite_accel + }, + { + "mahony_update_imu_accepts_valid_accel_magnitude", + test_mahony_update_imu_accepts_valid_accel_magnitude + }, }; TEST_INITIALIZE_TEST("mahony.c", tests); From a7709557cbe37feb2abdc540e321e6f158e3e288 Mon Sep 17 00:00:00 2001 From: Bjorn Date: Sat, 25 Jul 2026 16:01:19 -0700 Subject: [PATCH 5/7] Add integral correction and anti-windup to Mahony filter --- mahony/mahony.c | 96 ++++++- test/mahony/test_mahony.c | 558 ++++++++++++++++++++++++++++++++++++++ 2 files changed, 644 insertions(+), 10 deletions(-) diff --git a/mahony/mahony.c b/mahony/mahony.c index 9adf520..7b4037b 100644 --- a/mahony/mahony.c +++ b/mahony/mahony.c @@ -31,6 +31,13 @@ #define MAHONY_ACCEL_MIN_MAGNITUDE (0.85f * GRAVITY) #define MAHONY_ACCEL_MAX_MAGNITUDE (1.15f * GRAVITY) +/* + * Limits each integral correction component to prevent windup. The value is + * expressed as an angular-rate correction in radians per second and should be + * tuned using sensor characterization and flight data. + */ +#define MAHONY_INTEGRAL_LIMIT_RAD_S 0.25f + /*------------------------------------------------------------------------------ Private Functions ------------------------------------------------------------------------------*/ @@ -53,7 +60,6 @@ return } /* mahony_quat_is_finite */ - /** * @brief Determines whether every vector component is finite. */ @@ -85,7 +91,6 @@ return sqrtf } /* vector_magnitude */ - static bool vector_normalize ( VECTOR_3F *vector @@ -118,6 +123,26 @@ return true; } /* vector_normalize */ +static float clamp_float + ( + float value, + float minimum, + float maximum + ) +{ +if ( value < minimum ) + { + return minimum; + } + +if ( value > maximum ) + { + return maximum; + } + +return value; + +} /* clamp_float */ static VECTOR_3F vector_cross ( @@ -383,23 +408,74 @@ if ( apply_accel ) * estimated gravity direction toward the measured direction. */ attitude_error = vector_cross + ( + accel_body, + gravity_estimated_body + ); + + /* + * Accumulate persistent attitude error as a gyro-rate correction. Integral + * feedback is updated only while accelerometer feedback is both permitted and + * valid, preventing high-dynamic or corrupted measurements from winding up + * the correction state. + */ + if ( filter->integral_gain > 0.0f ) + { + filter->integral_error.x += + filter->integral_gain * + attitude_error.x * + delta_time_s; + + filter->integral_error.y += + filter->integral_gain * + attitude_error.y * + delta_time_s; + + filter->integral_error.z += + filter->integral_gain * + attitude_error.z * + delta_time_s; + + filter->integral_error.x = clamp_float ( - accel_body, - gravity_estimated_body + filter->integral_error.x, + -MAHONY_INTEGRAL_LIMIT_RAD_S, + MAHONY_INTEGRAL_LIMIT_RAD_S ); - proportional_correction = vector_scale + filter->integral_error.y = clamp_float ( - attitude_error, - filter->proportional_gain + filter->integral_error.y, + -MAHONY_INTEGRAL_LIMIT_RAD_S, + MAHONY_INTEGRAL_LIMIT_RAD_S ); - gyro_corrected = vector_add + filter->integral_error.z = clamp_float ( - gyro_corrected, - proportional_correction + filter->integral_error.z, + -MAHONY_INTEGRAL_LIMIT_RAD_S, + MAHONY_INTEGRAL_LIMIT_RAD_S ); } + + proportional_correction = vector_scale + ( + attitude_error, + filter->proportional_gain + ); + + gyro_corrected = vector_add + ( + gyro_corrected, + proportional_correction + ); + + gyro_corrected = vector_add + ( + gyro_corrected, + filter->integral_error + ); + } } return mahony_update_gyro diff --git a/test/mahony/test_mahony.c b/test/mahony/test_mahony.c index faabae6..684d914 100644 --- a/test/mahony/test_mahony.c +++ b/test/mahony/test_mahony.c @@ -2003,6 +2003,540 @@ assert_quat_components } /* test_mahony_update_imu_rejects_high_accel_magnitude */ +/** + * @brief Verifies that zero integral gain prevents integral accumulation. + * + * A valid tilt error is supplied repeatedly, but Ki is zero. The stored + * integral correction must remain zero. + */ +void test_mahony_integral_zero_gain_does_not_accumulate + ( + void + ) +{ +int32_t index; + +MAHONY_FILTER filter; + +QUAT initial_attitude = eul_to_quat + ( + 0.0f, + 0.0f, + deg_to_rad(10.0f) + ); + +VECTOR_3F zero_gyro = + { + .x = 0.0f, + .y = 0.0f, + .z = 0.0f + }; + +VECTOR_3F valid_accel = + { + .x = 0.0f, + .y = 0.0f, + .z = GRAVITY + }; + +TEST_ASSERT_TRUE + ( + "Mahony initialization succeeds", + mahony_init + ( + &filter, + initial_attitude, + 1.0f, + 0.0f + ) + ); + +for ( index = 0; index < 1000; index++ ) + { + TEST_ASSERT_TRUE + ( + "Zero-Ki update succeeds", + mahony_update_imu + ( + &filter, + zero_gyro, + valid_accel, + 0.001f, + true + ) + ); + } + +TEST_ASSERT_EQ_FLOAT + ( + "Zero Ki preserves integral X", + filter.integral_error.x, + 0.0f + ); + +TEST_ASSERT_EQ_FLOAT + ( + "Zero Ki preserves integral Y", + filter.integral_error.y, + 0.0f + ); + +TEST_ASSERT_EQ_FLOAT + ( + "Zero Ki preserves integral Z", + filter.integral_error.z, + 0.0f + ); + +} /* test_mahony_integral_zero_gain_does_not_accumulate */ + +/** + * @brief Verifies that valid accelerometer feedback accumulates integral error. + * + * The filter begins with a roll error, zero gyro rate, and valid gravity. + * A nonzero Ki should accumulate a correction about the roll axis. + */ +void test_mahony_integral_valid_error_accumulates + ( + void + ) +{ +MAHONY_FILTER filter; + +QUAT initial_attitude = eul_to_quat + ( + 0.0f, + 0.0f, + deg_to_rad(10.0f) + ); + +VECTOR_3F zero_gyro = + { + .x = 0.0f, + .y = 0.0f, + .z = 0.0f + }; + +VECTOR_3F valid_accel = + { + .x = 0.0f, + .y = 0.0f, + .z = GRAVITY + }; + +TEST_ASSERT_TRUE + ( + "Mahony initialization succeeds", + mahony_init + ( + &filter, + initial_attitude, + 0.0f, + 0.5f + ) + ); + +TEST_ASSERT_TRUE + ( + "Integral update succeeds", + mahony_update_imu + ( + &filter, + zero_gyro, + valid_accel, + 0.1f, + true + ) + ); + +TEST_ASSERT_TRUE + ( + "Roll error accumulates integral correction", + fabsf(filter.integral_error.x) > 0.0f + ); + +TEST_ASSERT_TRUE + ( + "Pure roll error produces negligible integral Y", + fabsf(filter.integral_error.y) < TEST_TOLERANCE + ); + +TEST_ASSERT_TRUE + ( + "Pure roll error produces negligible integral Z", + fabsf(filter.integral_error.z) < TEST_TOLERANCE + ); + +} /* test_mahony_integral_valid_error_accumulates */ + +/** + * @brief Verifies that disabling accelerometer feedback prevents windup. + * + * Even with valid gravity and a tilt error, use_accel=false must prevent the + * integral state from accumulating. + */ +void test_mahony_integral_disabled_accel_does_not_accumulate + ( + void + ) +{ +int32_t index; + +MAHONY_FILTER filter; + +QUAT initial_attitude = eul_to_quat + ( + 0.0f, + 0.0f, + deg_to_rad(10.0f) + ); + +VECTOR_3F zero_gyro = + { + .x = 0.0f, + .y = 0.0f, + .z = 0.0f + }; + +VECTOR_3F valid_accel = + { + .x = 0.0f, + .y = 0.0f, + .z = GRAVITY + }; + +TEST_ASSERT_TRUE + ( + "Mahony initialization succeeds", + mahony_init + ( + &filter, + initial_attitude, + 1.0f, + 0.5f + ) + ); + +for ( index = 0; index < 1000; index++ ) + { + TEST_ASSERT_TRUE + ( + "Disabled-accelerometer update succeeds", + mahony_update_imu + ( + &filter, + zero_gyro, + valid_accel, + 0.001f, + false + ) + ); + } + +TEST_ASSERT_EQ_FLOAT + ( + "Disabled accelerometer preserves integral X", + filter.integral_error.x, + 0.0f + ); + +TEST_ASSERT_EQ_FLOAT + ( + "Disabled accelerometer preserves integral Y", + filter.integral_error.y, + 0.0f + ); + +TEST_ASSERT_EQ_FLOAT + ( + "Disabled accelerometer preserves integral Z", + filter.integral_error.z, + 0.0f + ); + +} /* test_mahony_integral_disabled_accel_does_not_accumulate */ + +/** + * @brief Verifies that invalid acceleration does not accumulate integral error. + * + * A two-g acceleration exceeds the configured validity range and must therefore + * be excluded from integral feedback. + */ +void test_mahony_integral_invalid_accel_does_not_accumulate + ( + void + ) +{ +int32_t index; + +MAHONY_FILTER filter; + +QUAT initial_attitude = eul_to_quat + ( + 0.0f, + 0.0f, + deg_to_rad(10.0f) + ); + +VECTOR_3F zero_gyro = + { + .x = 0.0f, + .y = 0.0f, + .z = 0.0f + }; + +VECTOR_3F invalid_accel = + { + .x = 0.0f, + .y = 0.0f, + .z = 2.0f * GRAVITY + }; + +TEST_ASSERT_TRUE + ( + "Mahony initialization succeeds", + mahony_init + ( + &filter, + initial_attitude, + 1.0f, + 0.5f + ) + ); + +for ( index = 0; index < 1000; index++ ) + { + TEST_ASSERT_TRUE + ( + "Invalid-accelerometer update succeeds", + mahony_update_imu + ( + &filter, + zero_gyro, + invalid_accel, + 0.001f, + true + ) + ); + } + +TEST_ASSERT_EQ_FLOAT + ( + "Invalid acceleration preserves integral X", + filter.integral_error.x, + 0.0f + ); + +TEST_ASSERT_EQ_FLOAT + ( + "Invalid acceleration preserves integral Y", + filter.integral_error.y, + 0.0f + ); + +TEST_ASSERT_EQ_FLOAT + ( + "Invalid acceleration preserves integral Z", + filter.integral_error.z, + 0.0f + ); + +} /* test_mahony_integral_invalid_accel_does_not_accumulate */ + +/** + * @brief Verifies that integral correction is limited by anti-windup. + * + * A large integral gain and large roll error would otherwise produce an + * excessive stored angular-rate correction. + */ +void test_mahony_integral_is_limited + ( + void + ) +{ +const float expected_limit_rad_s = 0.25f; + +MAHONY_FILTER filter; + +QUAT initial_attitude = eul_to_quat + ( + 0.0f, + 0.0f, + deg_to_rad(90.0f) + ); + +VECTOR_3F zero_gyro = + { + .x = 0.0f, + .y = 0.0f, + .z = 0.0f + }; + +VECTOR_3F valid_accel = + { + .x = 0.0f, + .y = 0.0f, + .z = GRAVITY + }; + +TEST_ASSERT_TRUE + ( + "Mahony initialization succeeds", + mahony_init + ( + &filter, + initial_attitude, + 0.0f, + 100.0f + ) + ); + +TEST_ASSERT_TRUE + ( + "High-integral-gain update succeeds", + mahony_update_imu + ( + &filter, + zero_gyro, + valid_accel, + 1.0f, + true + ) + ); + +TEST_ASSERT_TRUE + ( + "Integral X does not exceed anti-windup limit", + fabsf(filter.integral_error.x) <= + expected_limit_rad_s + TEST_TOLERANCE + ); + +TEST_ASSERT_TRUE + ( + "Integral Y does not exceed anti-windup limit", + fabsf(filter.integral_error.y) <= + expected_limit_rad_s + TEST_TOLERANCE + ); + +TEST_ASSERT_TRUE + ( + "Integral Z does not exceed anti-windup limit", + fabsf(filter.integral_error.z) <= + expected_limit_rad_s + TEST_TOLERANCE + ); + +TEST_ASSERT_TRUE + ( + "Large roll error reaches integral limit", + fabsf + ( + fabsf(filter.integral_error.x) - + expected_limit_rad_s + ) < + TEST_TOLERANCE + ); + +} /* test_mahony_integral_is_limited */ + +/** + * @brief Verifies that accumulated integral error affects attitude propagation. + * + * Two filters begin with the same roll error. One has Ki=0 and one has Ki>0. + * With Kp=0 and zero measured gyro, only the filter with integral feedback + * should change its attitude. + */ +void test_mahony_integral_correction_affects_attitude + ( + void + ) +{ +MAHONY_FILTER no_integral_filter; +MAHONY_FILTER integral_filter; + +QUAT initial_attitude = eul_to_quat + ( + 0.0f, + 0.0f, + deg_to_rad(10.0f) + ); + +VECTOR_3F zero_gyro = + { + .x = 0.0f, + .y = 0.0f, + .z = 0.0f + }; + +VECTOR_3F valid_accel = + { + .x = 0.0f, + .y = 0.0f, + .z = GRAVITY + }; + +TEST_ASSERT_TRUE + ( + "No-integral filter initialization succeeds", + mahony_init + ( + &no_integral_filter, + initial_attitude, + 0.0f, + 0.0f + ) + ); + +TEST_ASSERT_TRUE + ( + "Integral filter initialization succeeds", + mahony_init + ( + &integral_filter, + initial_attitude, + 0.0f, + 1.0f + ) + ); + +TEST_ASSERT_TRUE + ( + "No-integral update succeeds", + mahony_update_imu + ( + &no_integral_filter, + zero_gyro, + valid_accel, + 0.1f, + true + ) + ); + +TEST_ASSERT_TRUE + ( + "Integral update succeeds", + mahony_update_imu + ( + &integral_filter, + zero_gyro, + valid_accel, + 0.1f, + true + ) + ); + +TEST_ASSERT_TRUE + ( + "Integral correction changes propagated attitude", + fabsf + ( + integral_filter.attitude.x - + no_integral_filter.attitude.x + ) > + 0.000001f + ); + +} /* test_mahony_integral_correction_affects_attitude */ + /*------------------------------------------------------------------------------ Main ------------------------------------------------------------------------------*/ @@ -2098,6 +2632,30 @@ unit_test tests[] = "mahony_update_imu_accepts_valid_accel_magnitude", test_mahony_update_imu_accepts_valid_accel_magnitude }, + { + "mahony_integral_zero_gain_does_not_accumulate", + test_mahony_integral_zero_gain_does_not_accumulate + }, + { + "mahony_integral_valid_error_accumulates", + test_mahony_integral_valid_error_accumulates + }, + { + "mahony_integral_disabled_accel_does_not_accumulate", + test_mahony_integral_disabled_accel_does_not_accumulate + }, + { + "mahony_integral_invalid_accel_does_not_accumulate", + test_mahony_integral_invalid_accel_does_not_accumulate + }, + { + "mahony_integral_is_limited", + test_mahony_integral_is_limited + }, + { + "mahony_integral_correction_affects_attitude", + test_mahony_integral_correction_affects_attitude + }, }; TEST_INITIALIZE_TEST("mahony.c", tests); From eaa16eddd6e346b5fc81c180b7cafea24a255b4d Mon Sep 17 00:00:00 2001 From: Bjorn Bengtsson Date: Sun, 26 Jul 2026 15:07:52 -0700 Subject: [PATCH 6/7] Integrate Mahony filter into sensor state estimation --- sensor/sensor.c | 187 ++++++++++++++++++++++++++++++++---------------- 1 file changed, 125 insertions(+), 62 deletions(-) diff --git a/sensor/sensor.c b/sensor/sensor.c index 79ba662..10c08c1 100644 --- a/sensor/sensor.c +++ b/sensor/sensor.c @@ -45,6 +45,20 @@ #include "usb.h" #include "sensor.h" #include "math_sdr.h" +#include "mahony.h" + +/*------------------------------------------------------------------------------ + Private Macros +------------------------------------------------------------------------------*/ + +/* + * Initial Mahony gains for firmware integration. + * + * Proportional correction is enabled conservatively. Integral correction + * remains disabled until the gains are tuned using stationary and flight data. + */ +#define SENSOR_MAHONY_KP 1.0f +#define SENSOR_MAHONY_KI 0.0f /*------------------------------------------------------------------------------ @@ -61,15 +75,20 @@ float velo_x_prev = 0.0f; float velo_y_prev = 0.0f; float velo_z_prev = 0.0f; -/* State estimation */ -QUAT attitude = { 1.0f, 0.0f, 0.0f, 0.0f }; - /*------------------------------------------------------------------------------ Static Variables ------------------------------------------------------------------------------*/ static MOUNT_ORIENTATION mount_orientation = MOUNT_ORIENTATION_IMU_INVERTED; /* Default assumption: antennta pointing up */ +/* + * Persistent attitude-filter state. This instance retains the quaternion and + * integral correction between consecutive IMU updates. + */ +static MAHONY_FILTER mahony_filter; + +/* Timestamp of the previous Mahony update in microseconds. */ +static uint64_t mahony_tick = 0; /*------------------------------------------------------------------------------ Internal function prototypes @@ -323,19 +342,42 @@ else * * *******************************************************************************/ void sensor_init - ( - PRESET_DATA* preset_data - ) + ( + PRESET_DATA* preset_data + ) { -imu_velo_tick = get_us_tick(); - -sensor_reset_velo(); +QUAT identity = + { + .w = 1.0f, + .x = 0.0f, + .y = 0.0f, + .z = 0.0f + }; float ax = preset_data->imu_offset.accel_x; float ay = preset_data->imu_offset.accel_y; float az = preset_data->imu_offset.accel_z; -attitude = quat_grav_attitude(ax, ay, az, attitude); +QUAT initial_attitude = quat_grav_attitude + ( + ax, + ay, + az, + identity + ); + +imu_velo_tick = get_us_tick(); +mahony_tick = imu_velo_tick; + +sensor_reset_velo(); + +(void)mahony_init + ( + &mahony_filter, + initial_attitude, + SENSOR_MAHONY_KP, + SENSOR_MAHONY_KI + ); } /* sensor_init */ @@ -412,63 +454,84 @@ mount_orientation = orientation; * Perform sensor fusion on imu converted data to get body rate * * * *******************************************************************************/ -static uint32_t last_tick = 0; + void sensor_body_state - ( - const IMU_CONVERTED* imu_converted, - STATE_ESTIMATION* state_estimate - ) + ( + const IMU_CONVERTED* imu_converted, + STATE_ESTIMATION* state_estimate + ) { -/* Determine delta T */ -uint32_t now_tick = HAL_GetTick(); -float dt = (now_tick - last_tick) / 1000.0f; -if ( dt <= 0.0f || dt > 1.0f ) - { - dt = 0.01f; - } -last_tick = now_tick; - -/* Copy IMU data for readability */ -// float ax = imu_converted->accel_x; -// float ay = imu_converted->accel_y; -// float az = imu_converted->accel_z; - -/* Raw gyro data in deg/s */ -float gx = imu_converted->gyro_x; -float gy = imu_converted->gyro_y; -float gz = imu_converted->gyro_z; - -/* Convert gyro to pure quaternion */ -QUAT q_gyro; /* Must be in radians */ -q_gyro.w = 0.0f; -q_gyro.x = deg_to_rad(gx); -q_gyro.y = deg_to_rad(gy); -q_gyro.z = deg_to_rad(gz); - -/* q_rate = 0.5 * attitude * q_gyro */ -QUAT q_rate = quat_mult(attitude, q_gyro); -q_rate = quat_scale(q_rate, 0.5f); - -/* Dead reckoing orientation by integrating gyro */ -/* attitude += dt * q_rate */ -QUAT rate_dt = quat_scale(q_rate, dt); -attitude = quat_add(attitude, rate_dt); - -/* Sensor fuson with gravity if not in flight */ -if ( get_fc_state() <= FC_STATE_LAUNCH_DETECT ) - { - // QUAT q_acc = quat_grav_attitude(ax, ay, az, attitude); - // gravity_comp_filter(&attitude, q_acc); - } +uint64_t current_tick; +uint64_t imu_tdelta; -/* Scale back to unit quaternion to avoid drift */ -attitude = quat_normalize(attitude); +float delta_time_s; -/* Store results */ -state_estimate->attitude = attitude; -state_estimate->roll_rate = gx; /* Rate in deg/s */ +bool use_accel; -} +VECTOR_3F gyro_body_rad_s; +VECTOR_3F accel_body_m_s2; + +/* + * Calculate elapsed time between attitude updates using the microsecond timer. + */ +current_tick = get_us_tick(); +imu_tdelta = current_tick - mahony_tick; + +delta_time_s = + (float)imu_tdelta / + (float)MICROSEC_PER_SEC; + +if ( mahony_tick == 0 || + delta_time_s <= 0.0f || + delta_time_s > 1.0f ) + { + delta_time_s = 0.01f; + } + +mahony_tick = current_tick; + +/* + * Converted gyro data is in degrees per second. Mahony requires radians per + * second. + */ +gyro_body_rad_s.x = deg_to_rad(imu_converted->gyro_x); +gyro_body_rad_s.y = deg_to_rad(imu_converted->gyro_y); +gyro_body_rad_s.z = deg_to_rad(imu_converted->gyro_z); + +/* + * Converted accelerometer data is already in meters per second squared. + */ +accel_body_m_s2.x = imu_converted->accel_x; +accel_body_m_s2.y = imu_converted->accel_y; +accel_body_m_s2.z = imu_converted->accel_z; + +/* + * Permit accelerometer correction only before powered flight. The Mahony + * filter still performs its own magnitude and finite-value checks. + */ +use_accel = + get_fc_state() <= FC_STATE_LAUNCH_DETECT; + +(void)mahony_update_imu + ( + &mahony_filter, + gyro_body_rad_s, + accel_body_m_s2, + delta_time_s, + use_accel + ); + +/* + * Store the filter's body-to-world quaternion as the system attitude estimate. + */ +state_estimate->attitude = mahony_filter.attitude; + +/* + * Preserve the existing public roll-rate units of degrees per second. + */ +state_estimate->roll_rate = imu_converted->gyro_x; + +} /* sensor_body_state */ /******************************************************************************* From 3e82cb8beac73df8d7ab6b3488bab7a12587afce Mon Sep 17 00:00:00 2001 From: Bjorn Bengtsson Date: Sun, 26 Jul 2026 19:01:47 -0700 Subject: [PATCH 7/7] Document Mahony gain tuning rationale --- sensor/sensor.c | 11 ++++++++++- 1 file changed, 10 insertions(+), 1 deletion(-) diff --git a/sensor/sensor.c b/sensor/sensor.c index 10c08c1..ab25898 100644 --- a/sensor/sensor.c +++ b/sensor/sensor.c @@ -56,6 +56,15 @@ * * Proportional correction is enabled conservatively. Integral correction * remains disabled until the gains are tuned using stationary and flight data. + * + * + * Proportional gain (KP) corrects gyro drift using the accelerometer. It is + * deliberately low to reduce overcorrection during flight. + * + * Integral gain (KI) learns persistent gyro bias over time. It remains + * disabled because an untuned integral term can accumulate incorrect + * corrections during vibration, launch acceleration, or invalid accelerometer + * data. */ #define SENSOR_MAHONY_KP 1.0f #define SENSOR_MAHONY_KI 0.0f @@ -82,7 +91,7 @@ float velo_z_prev = 0.0f; static MOUNT_ORIENTATION mount_orientation = MOUNT_ORIENTATION_IMU_INVERTED; /* Default assumption: antennta pointing up */ /* - * Persistent attitude-filter state. This instance retains the quaternion and + * Persistent attitude filter state. This instance retains the quaternion and * integral correction between consecutive IMU updates. */ static MAHONY_FILTER mahony_filter;