From acab7f4280f8d8a3f232f59baf50da9f104ea33a Mon Sep 17 00:00:00 2001 From: Bjorn Bengtsson Date: Sun, 26 Jul 2026 19:13:51 -0700 Subject: [PATCH 1/6] Move shared vector type to math module --- mahony/mahony.h | 10 ---------- math_sdr/math_sdr.h | 9 +++++++++ 2 files changed, 9 insertions(+), 10 deletions(-) diff --git a/mahony/mahony.h b/mahony/mahony.h index 38e1c48..e7dc278 100644 --- a/mahony/mahony.h +++ b/mahony/mahony.h @@ -30,16 +30,6 @@ extern "C" 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. */ diff --git a/math_sdr/math_sdr.h b/math_sdr/math_sdr.h index a884f2a..9ed7a65 100644 --- a/math_sdr/math_sdr.h +++ b/math_sdr/math_sdr.h @@ -57,6 +57,15 @@ typedef struct _QUAT float w, x, y, z; } QUAT; +/** + * @brief Three-dimensional floating-point vector. + */ +typedef struct _VECTOR_3F + { + float x; + float y; + float z; + } VECTOR_3F; /*------------------------------------------------------------------------------ Macros From 3b91d66ba6f433adecaecf0fbf46054955b606aa Mon Sep 17 00:00:00 2001 From: Bjorn Bengtsson Date: Sun, 26 Jul 2026 19:44:30 -0700 Subject: [PATCH 2/6] Start MEKF state and initialization --- mekf/mekf.c | 252 ++++++++++++++++++++++++++++++++++++++++++++++++++++ mekf/mekf.h | 206 ++++++++++++++++++++++++++++++++++++++++++ 2 files changed, 458 insertions(+) create mode 100644 mekf/mekf.c create mode 100644 mekf/mekf.h diff --git a/mekf/mekf.c b/mekf/mekf.c new file mode 100644 index 0000000..60ddf29 --- /dev/null +++ b/mekf/mekf.c @@ -0,0 +1,252 @@ +/******************************************************************************* + * + * FILE: + * mekf.c + * + * DESCRIPTION: + * Multiplicative Extended Kalman Filter attitude estimator implementation. + * + ******************************************************************************/ + +/*------------------------------------------------------------------------------ + Standard Includes + ------------------------------------------------------------------------------*/ +#include +#include + +/*------------------------------------------------------------------------------ + Project Includes + ------------------------------------------------------------------------------*/ +#include "mekf.h" + +/*------------------------------------------------------------------------------ + Private Functions + ------------------------------------------------------------------------------*/ + +/** + * @brief Determines whether every quaternion component is finite. + */ +static bool mekf_quat_is_finite + ( + QUAT quaternion + ) +{ +return + ( + isfinite(quaternion.w) && + isfinite(quaternion.x) && + isfinite(quaternion.y) && + isfinite(quaternion.z) + ); + +} /* mekf_quat_is_finite */ + +/** + * @brief Determines whether every vector component is finite. + */ +static bool mekf_vector_is_finite + ( + VECTOR_3F vector + ) +{ +return + ( + isfinite(vector.x) && + isfinite(vector.y) && + isfinite(vector.z) + ); + +} /* mekf_vector_is_finite */ + +/** + * @brief Determines whether a standard deviation and its variance are valid. + */ +static bool mekf_standard_deviation_is_valid + ( + float standard_deviation + ) +{ +float variance; + +if ( !isfinite(standard_deviation) || + standard_deviation < 0.0f ) + { + return false; + } + +variance = standard_deviation * standard_deviation; + +return isfinite(variance); + +} /* mekf_standard_deviation_is_valid */ + +/** + * @brief Determines whether all components are valid standard deviations. + */ +static bool mekf_standard_deviation_vector_is_valid + ( + VECTOR_3F standard_deviation + ) +{ +return + ( + mekf_standard_deviation_is_valid(standard_deviation.x) && + mekf_standard_deviation_is_valid(standard_deviation.y) && + mekf_standard_deviation_is_valid(standard_deviation.z) + ); + +} /* mekf_standard_deviation_vector_is_valid */ + +/** + * @brief Validates MEKF initialization and process-noise configuration. + */ +static bool mekf_config_is_valid + ( + const MEKF_CONFIG *config + ) +{ +if ( config == NULL ) + { + return false; + } + +if ( !mekf_standard_deviation_vector_is_valid + ( + config->initial_attitude_std_rad + ) ) + { + return false; + } + +if ( !mekf_standard_deviation_vector_is_valid + ( + config->initial_gyro_bias_std_rad_s + ) ) + { + return false; + } + +if ( !mekf_standard_deviation_is_valid + ( + config->gyro_noise_density_rad_s_sqrt_hz + ) ) + { + return false; + } + +if ( !mekf_standard_deviation_is_valid + ( + config->gyro_bias_random_walk_rad_s2_sqrt_hz + ) ) + { + return false; + } + +if ( !isfinite(config->maximum_delta_time_s) || + config->maximum_delta_time_s <= 0.0f ) + { + return false; + } + +return true; + +} /* mekf_config_is_valid */ + +/*------------------------------------------------------------------------------ + Public Functions + ------------------------------------------------------------------------------*/ + +bool mekf_init + ( + MEKF_FILTER *filter, + QUAT initial_attitude, + VECTOR_3F initial_gyro_bias_rad_s, + const MEKF_CONFIG *config + ) +{ +unsigned int row; +unsigned int column; + +if ( filter == NULL ) + { + return false; + } + +if ( !mekf_quat_is_finite(initial_attitude) ) + { + return false; + } + +if ( !mekf_vector_is_finite(initial_gyro_bias_rad_s) ) + { + return false; + } + +if ( !mekf_config_is_valid(config) ) + { + return false; + } + +/* + * Store the normalized nominal body-to-world attitude and the nominal + * body-frame gyro-bias estimate. + */ +filter->attitude = quat_normalize(initial_attitude); +filter->gyro_bias_rad_s = initial_gyro_bias_rad_s; +filter->config = *config; + +/* + * The MEKF error state begins with zero mean. Initialize its covariance as a + * diagonal matrix using the configured per-axis standard deviations. + */ +for ( row = 0U; row < MEKF_ERROR_STATE_DIM; row++ ) + { + for ( column = 0U; column < MEKF_ERROR_STATE_DIM; column++ ) + { + filter->covariance[row][column] = 0.0f; + } + } + +filter->covariance + [MEKF_ATTITUDE_ERROR_X] + [MEKF_ATTITUDE_ERROR_X] = + config->initial_attitude_std_rad.x * + config->initial_attitude_std_rad.x; + +filter->covariance + [MEKF_ATTITUDE_ERROR_Y] + [MEKF_ATTITUDE_ERROR_Y] = + config->initial_attitude_std_rad.y * + config->initial_attitude_std_rad.y; + +filter->covariance + [MEKF_ATTITUDE_ERROR_Z] + [MEKF_ATTITUDE_ERROR_Z] = + config->initial_attitude_std_rad.z * + config->initial_attitude_std_rad.z; + +filter->covariance + [MEKF_GYRO_BIAS_ERROR_X] + [MEKF_GYRO_BIAS_ERROR_X] = + config->initial_gyro_bias_std_rad_s.x * + config->initial_gyro_bias_std_rad_s.x; + +filter->covariance + [MEKF_GYRO_BIAS_ERROR_Y] + [MEKF_GYRO_BIAS_ERROR_Y] = + config->initial_gyro_bias_std_rad_s.y * + config->initial_gyro_bias_std_rad_s.y; + +filter->covariance + [MEKF_GYRO_BIAS_ERROR_Z] + [MEKF_GYRO_BIAS_ERROR_Z] = + config->initial_gyro_bias_std_rad_s.z * + config->initial_gyro_bias_std_rad_s.z; + +return true; + +} /* mekf_init */ + +/******************************************************************************* + * END OF FILE + ******************************************************************************/ \ No newline at end of file diff --git a/mekf/mekf.h b/mekf/mekf.h new file mode 100644 index 0000000..9f54705 --- /dev/null +++ b/mekf/mekf.h @@ -0,0 +1,206 @@ +/******************************************************************************* + * + * FILE: + * mekf.h + * + * DESCRIPTION: + * Multiplicative Extended Kalman Filter attitude estimator interface. + * + ******************************************************************************/ + +#ifndef MEKF_H +#define MEKF_H + +#ifdef __cplusplus +extern "C" +{ +#endif + +/*------------------------------------------------------------------------------ + Standard Includes + ------------------------------------------------------------------------------*/ +#include + +/*------------------------------------------------------------------------------ + Project Includes + ------------------------------------------------------------------------------*/ +#include "math_sdr.h" + +/*------------------------------------------------------------------------------ + Macros + ------------------------------------------------------------------------------*/ + +/** + * @brief Number of elements in the MEKF error state. + */ +#define MEKF_ERROR_STATE_DIM 6U + +/*------------------------------------------------------------------------------ + Typedefs + ------------------------------------------------------------------------------*/ + +/** + * @brief Indices into the six-state MEKF error vector. + * + * The multiplicative error state is: + * + * delta_x = + * [ + * delta_theta_x, + * delta_theta_y, + * delta_theta_z, + * delta_bias_x, + * delta_bias_y, + * delta_bias_z + * ] + * + * The attitude error is a local body-frame rotation in radians. The gyro-bias + * error is expressed in body-frame radians per second. + */ +typedef enum _MEKF_ERROR_STATE_INDEX + { + MEKF_ATTITUDE_ERROR_X = 0, + MEKF_ATTITUDE_ERROR_Y, + MEKF_ATTITUDE_ERROR_Z, + MEKF_GYRO_BIAS_ERROR_X, + MEKF_GYRO_BIAS_ERROR_Y, + MEKF_GYRO_BIAS_ERROR_Z + } MEKF_ERROR_STATE_INDEX; + +/** + * @brief Initial uncertainty and gyro prediction configuration. + */ +typedef struct _MEKF_CONFIG + { + /** + * Initial one-sigma local attitude uncertainty in radians. + */ + VECTOR_3F initial_attitude_std_rad; + + /** + * Initial one-sigma gyro-bias uncertainty in radians per second. + */ + VECTOR_3F initial_gyro_bias_std_rad_s; + + /** + * Continuous gyroscope white-noise density in radians per second per + * square-root hertz. + */ + float gyro_noise_density_rad_s_sqrt_hz; + + /** + * Continuous gyro-bias random-walk density in radians per second squared + * per square-root hertz. + */ + float gyro_bias_random_walk_rad_s2_sqrt_hz; + + /** + * Maximum valid gyro prediction timestep in seconds. + */ + float maximum_delta_time_s; + + } MEKF_CONFIG; + +/** + * @brief Nominal state, covariance, and configuration for a six-state MEKF. + * + * The nominal attitude is a body-to-world quaternion: + * + * vector_world = + * attitude * vector_body * conjugate(attitude) + * + * A right-multiplicative local attitude error is used: + * + * attitude_true = + * attitude_nominal * delta_attitude + * + * The covariance represents uncertainty in the local six-state error vector, + * not uncertainty in the four quaternion components: + * + * P = E[delta_x * transpose(delta_x)] + */ +typedef struct _MEKF_FILTER + { + /** + * Nominal body-to-world attitude quaternion. + */ + QUAT attitude; + + /** + * Nominal body-frame gyro-bias estimate in radians per second. + */ + VECTOR_3F gyro_bias_rad_s; + + /** + * Six-by-six error-state covariance matrix. + */ + float covariance[MEKF_ERROR_STATE_DIM][MEKF_ERROR_STATE_DIM]; + + /** + * Initial uncertainty, process-noise, and timestep configuration. + */ + MEKF_CONFIG config; + + } MEKF_FILTER; + +/*------------------------------------------------------------------------------ + Function Prototypes + ------------------------------------------------------------------------------*/ + +/** + * @brief Initializes a six-state attitude and gyro-bias MEKF. + * + * The initial attitude is normalized. The initial covariance is diagonal and + * is constructed from the squared per-axis standard deviations in the + * configuration. + * + * @param filter Filter instance to initialize. + * @param initial_attitude Initial body-to-world attitude quaternion. + * @param initial_gyro_bias_rad_s Initial body-frame gyro-bias estimate in + * radians per second. + * @param config Initial uncertainty, process-noise, and timestep configuration. + * + * @return true when initialization succeeds; otherwise false. + */ +bool mekf_init + ( + MEKF_FILTER *filter, + QUAT initial_attitude, + VECTOR_3F initial_gyro_bias_rad_s, + const MEKF_CONFIG *config + ); + +/** + * @brief Predicts nominal attitude and covariance using a gyro measurement. + * + * The gyro measurement must be expressed in body-frame radians per second. + * The stored gyro-bias estimate is subtracted before attitude propagation. + * + * The attitude quaternion is propagated using right multiplication: + * + * attitude_new = + * normalize(attitude_old * delta_attitude) + * + * @param filter Initialized filter instance. + * @param gyro_body_rad_s Body-frame gyroscope measurement in radians per + * second. + * @param delta_time_s Elapsed time in seconds. + * + * @return true when prediction succeeds; otherwise false. + */ +bool mekf_predict + ( + MEKF_FILTER *filter, + VECTOR_3F gyro_body_rad_s, + float delta_time_s + ); + +#ifdef __cplusplus +} +#endif + +#endif /* MEKF_H */ + +/******************************************************************************* + * END OF FILE + ******************************************************************************/ \ No newline at end of file From 90ca414e3870d55ff868627f8d80283785853848 Mon Sep 17 00:00:00 2001 From: Bjorn Bengtsson Date: Mon, 27 Jul 2026 12:20:45 -0700 Subject: [PATCH 3/6] Add MEKF initialization unit tests --- test/mekf/.gitignore | 6 + test/mekf/Makefile | 106 ++++++ test/mekf/main.h | 8 + test/mekf/test_mekf.c | 833 ++++++++++++++++++++++++++++++++++++++++++ 4 files changed, 953 insertions(+) create mode 100644 test/mekf/.gitignore create mode 100644 test/mekf/Makefile create mode 100644 test/mekf/main.h create mode 100644 test/mekf/test_mekf.c diff --git a/test/mekf/.gitignore b/test/mekf/.gitignore new file mode 100644 index 0000000..9bf4711 --- /dev/null +++ b/test/mekf/.gitignore @@ -0,0 +1,6 @@ +build/ +coverage/ +results.txt +*.gcda +*.gcno +*.gcov diff --git a/test/mekf/Makefile b/test/mekf/Makefile new file mode 100644 index 0000000..03c5280 --- /dev/null +++ b/test/mekf/Makefile @@ -0,0 +1,106 @@ +################################################################ +# +# MEKF unit tests (based on gcc) +# +################################################################ + +################################################################ +# target +################################################################ +TARGET = mekf + +################################################################ +# build variables +################################################################ +DEBUG ?= 0 +OPT = -Og + +################################################################ +# paths +################################################################ +BUILD_DIR = build +ROOT_DIR = ../.. + +################################################################ +# source +################################################################ +TEST_SOURCES = \ +test_mekf.c + +COV_C_SOURCES = \ +$(ROOT_DIR)/mekf/mekf.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)/mekf \ +-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)/mekf/mekf.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/mekf/main.h b/test/mekf/main.h new file mode 100644 index 0000000..a16ba3f --- /dev/null +++ b/test/mekf/main.h @@ -0,0 +1,8 @@ +#ifndef TEST_MEKF_MAIN_H +#define TEST_MEKF_MAIN_H + +#include +#include +#include + +#endif /* TEST_MEKF_MAIN_H */ \ No newline at end of file diff --git a/test/mekf/test_mekf.c b/test/mekf/test_mekf.c new file mode 100644 index 0000000..a294108 --- /dev/null +++ b/test/mekf/test_mekf.c @@ -0,0 +1,833 @@ +/******************************************************************************* + * + * FILE: + * test_mekf.c + * + * DESCRIPTION: + * Unit tests for the Multiplicative Extended Kalman Filter. + * + ******************************************************************************/ + +/*------------------------------------------------------------------------------ + Standard Includes + ------------------------------------------------------------------------------*/ +#include +#include +#include + +/*------------------------------------------------------------------------------ + Project Includes + ------------------------------------------------------------------------------*/ +#include "mekf.h" +#include "sdrtf_pub.h" + +/*------------------------------------------------------------------------------ + Test Helpers + ------------------------------------------------------------------------------*/ + +/** + * @brief Creates a valid configuration for MEKF unit tests. + */ +static MEKF_CONFIG make_valid_config + ( + void + ) +{ +MEKF_CONFIG config = + { + .initial_attitude_std_rad = + { + .x = 0.10f, + .y = 0.20f, + .z = 0.30f + }, + .initial_gyro_bias_std_rad_s = + { + .x = 0.01f, + .y = 0.02f, + .z = 0.03f + }, + .gyro_noise_density_rad_s_sqrt_hz = 0.005f, + .gyro_bias_random_walk_rad_s2_sqrt_hz = 0.0001f, + .maximum_delta_time_s = 0.10f + }; + +return config; + +} /* make_valid_config */ + +/** + * @brief Checks all four quaternion components. + */ +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 */ + +/** + * @brief Checks all three vector components. + */ +static void assert_vector_components + ( + const char *description, + VECTOR_3F actual, + VECTOR_3F expected + ) +{ +TEST_begin_nested_case(description); + +TEST_ASSERT_EQ_FLOAT("Vector x component", actual.x, expected.x); +TEST_ASSERT_EQ_FLOAT("Vector y component", actual.y, expected.y); +TEST_ASSERT_EQ_FLOAT("Vector z component", actual.z, expected.z); + +TEST_end_nested_case(); + +} /* assert_vector_components */ + +/*------------------------------------------------------------------------------ + Initialization Tests + ------------------------------------------------------------------------------*/ + +/** + * @brief Verifies identity-attitude and gyro-bias initialization. + */ +void test_mekf_init_identity_and_bias + ( + void + ) +{ +MEKF_FILTER filter; +MEKF_CONFIG config = make_valid_config(); + +QUAT identity = + { + .w = 1.0f, + .x = 0.0f, + .y = 0.0f, + .z = 0.0f + }; + +VECTOR_3F initial_bias = + { + .x = 0.01f, + .y = -0.02f, + .z = 0.03f + }; + +TEST_ASSERT_TRUE + ( + "MEKF initialization succeeds", + mekf_init + ( + &filter, + identity, + initial_bias, + &config + ) + ); + +assert_quat_components + ( + "Identity attitude is preserved", + filter.attitude, + identity + ); + +assert_vector_components + ( + "Initial gyro bias is preserved", + filter.gyro_bias_rad_s, + initial_bias + ); + +} /* test_mekf_init_identity_and_bias */ + +/** + * @brief Verifies that initialization normalizes the nominal quaternion. + */ +void test_mekf_init_normalizes_attitude + ( + void + ) +{ +MEKF_FILTER filter; +MEKF_CONFIG config = make_valid_config(); + +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 + }; + +VECTOR_3F zero_bias = { 0.0f, 0.0f, 0.0f }; + +TEST_ASSERT_TRUE + ( + "MEKF initialization succeeds", + mekf_init + ( + &filter, + initial_attitude, + zero_bias, + &config + ) + ); + +assert_quat_components + ( + "Initial attitude is normalized", + filter.attitude, + expected + ); + +} /* test_mekf_init_normalizes_attitude */ + +/** + * @brief Verifies the shared zero-quaternion identity fallback. + */ +void test_mekf_init_zero_quaternion_uses_identity + ( + void + ) +{ +MEKF_FILTER filter; +MEKF_CONFIG config = make_valid_config(); + +QUAT zero_quaternion = { 0.0f, 0.0f, 0.0f, 0.0f }; +QUAT identity = { 1.0f, 0.0f, 0.0f, 0.0f }; +VECTOR_3F zero_bias = { 0.0f, 0.0f, 0.0f }; + +TEST_ASSERT_TRUE + ( + "Zero-quaternion initialization succeeds", + mekf_init + ( + &filter, + zero_quaternion, + zero_bias, + &config + ) + ); + +assert_quat_components + ( + "Zero quaternion uses identity", + filter.attitude, + identity + ); + +} /* test_mekf_init_zero_quaternion_uses_identity */ + +/** + * @brief Verifies diagonal covariance initialization. + */ +void test_mekf_init_sets_diagonal_covariance + ( + void + ) +{ +unsigned int row; +unsigned int column; + +MEKF_FILTER filter; +MEKF_CONFIG config = make_valid_config(); + +QUAT identity = { 1.0f, 0.0f, 0.0f, 0.0f }; +VECTOR_3F zero_bias = { 0.0f, 0.0f, 0.0f }; + +TEST_ASSERT_TRUE + ( + "MEKF initialization succeeds", + mekf_init + ( + &filter, + identity, + zero_bias, + &config + ) + ); + +TEST_ASSERT_EQ_FLOAT + ( + "Attitude X variance", + filter.covariance[MEKF_ATTITUDE_ERROR_X][MEKF_ATTITUDE_ERROR_X], + 0.10f * 0.10f + ); + +TEST_ASSERT_EQ_FLOAT + ( + "Attitude Y variance", + filter.covariance[MEKF_ATTITUDE_ERROR_Y][MEKF_ATTITUDE_ERROR_Y], + 0.20f * 0.20f + ); + +TEST_ASSERT_EQ_FLOAT + ( + "Attitude Z variance", + filter.covariance[MEKF_ATTITUDE_ERROR_Z][MEKF_ATTITUDE_ERROR_Z], + 0.30f * 0.30f + ); + +TEST_ASSERT_EQ_FLOAT + ( + "Bias X variance", + filter.covariance[MEKF_GYRO_BIAS_ERROR_X][MEKF_GYRO_BIAS_ERROR_X], + 0.01f * 0.01f + ); + +TEST_ASSERT_EQ_FLOAT + ( + "Bias Y variance", + filter.covariance[MEKF_GYRO_BIAS_ERROR_Y][MEKF_GYRO_BIAS_ERROR_Y], + 0.02f * 0.02f + ); + +TEST_ASSERT_EQ_FLOAT + ( + "Bias Z variance", + filter.covariance[MEKF_GYRO_BIAS_ERROR_Z][MEKF_GYRO_BIAS_ERROR_Z], + 0.03f * 0.03f + ); + +for ( row = 0U; row < MEKF_ERROR_STATE_DIM; row++ ) + { + for ( column = 0U; column < MEKF_ERROR_STATE_DIM; column++ ) + { + if ( row != column ) + { + TEST_ASSERT_EQ_FLOAT + ( + "Initial cross covariance is zero", + filter.covariance[row][column], + 0.0f + ); + } + } + } + +} /* test_mekf_init_sets_diagonal_covariance */ + +/** + * @brief Verifies that prediction configuration is copied into the filter. + */ +void test_mekf_init_copies_config + ( + void + ) +{ +MEKF_FILTER filter; +MEKF_CONFIG config = make_valid_config(); + +QUAT identity = { 1.0f, 0.0f, 0.0f, 0.0f }; +VECTOR_3F zero_bias = { 0.0f, 0.0f, 0.0f }; + +TEST_ASSERT_TRUE + ( + "MEKF initialization succeeds", + mekf_init + ( + &filter, + identity, + zero_bias, + &config + ) + ); + +TEST_ASSERT_EQ_FLOAT + ( + "Gyro noise density is copied", + filter.config.gyro_noise_density_rad_s_sqrt_hz, + config.gyro_noise_density_rad_s_sqrt_hz + ); + +TEST_ASSERT_EQ_FLOAT + ( + "Bias random walk is copied", + filter.config.gyro_bias_random_walk_rad_s2_sqrt_hz, + config.gyro_bias_random_walk_rad_s2_sqrt_hz + ); + +TEST_ASSERT_EQ_FLOAT + ( + "Maximum timestep is copied", + filter.config.maximum_delta_time_s, + config.maximum_delta_time_s + ); + +} /* test_mekf_init_copies_config */ + +/** + * @brief Verifies that null filter and configuration pointers are rejected. + */ +void test_mekf_init_rejects_null_pointers + ( + void + ) +{ +MEKF_FILTER filter; +MEKF_CONFIG config = make_valid_config(); + +QUAT identity = { 1.0f, 0.0f, 0.0f, 0.0f }; +VECTOR_3F zero_bias = { 0.0f, 0.0f, 0.0f }; + +TEST_ASSERT_TRUE + ( + "Null filter is rejected", + !mekf_init + ( + NULL, + identity, + zero_bias, + &config + ) + ); + +TEST_ASSERT_TRUE + ( + "Null configuration is rejected", + !mekf_init + ( + &filter, + identity, + zero_bias, + NULL + ) + ); + +} /* test_mekf_init_rejects_null_pointers */ + +/** + * @brief Verifies rejection of nonfinite nominal-state inputs. + */ +void test_mekf_init_rejects_nonfinite_state + ( + void + ) +{ +MEKF_FILTER filter; +MEKF_CONFIG config = make_valid_config(); + +QUAT invalid_attitude = { NAN, 0.0f, 0.0f, 0.0f }; +QUAT identity = { 1.0f, 0.0f, 0.0f, 0.0f }; + +VECTOR_3F zero_bias = { 0.0f, 0.0f, 0.0f }; +VECTOR_3F invalid_bias = { 0.0f, INFINITY, 0.0f }; + +TEST_ASSERT_TRUE + ( + "Nonfinite attitude is rejected", + !mekf_init + ( + &filter, + invalid_attitude, + zero_bias, + &config + ) + ); + +TEST_ASSERT_TRUE + ( + "Nonfinite gyro bias is rejected", + !mekf_init + ( + &filter, + identity, + invalid_bias, + &config + ) + ); + +} /* test_mekf_init_rejects_nonfinite_state */ + +/** + * @brief Verifies rejection of invalid initial uncertainty. + */ +void test_mekf_init_rejects_invalid_uncertainty + ( + void + ) +{ +MEKF_FILTER filter; +MEKF_CONFIG config; + +QUAT identity = { 1.0f, 0.0f, 0.0f, 0.0f }; +VECTOR_3F zero_bias = { 0.0f, 0.0f, 0.0f }; + +config = make_valid_config(); +config.initial_attitude_std_rad.x = -0.10f; + +TEST_ASSERT_TRUE + ( + "Negative attitude uncertainty is rejected", + !mekf_init + ( + &filter, + identity, + zero_bias, + &config + ) + ); + +config = make_valid_config(); +config.initial_gyro_bias_std_rad_s.y = NAN; + +TEST_ASSERT_TRUE + ( + "Nonfinite bias uncertainty is rejected", + !mekf_init + ( + &filter, + identity, + zero_bias, + &config + ) + ); + +config = make_valid_config(); +config.initial_attitude_std_rad.z = FLT_MAX; + +TEST_ASSERT_TRUE + ( + "Uncertainty variance overflow is rejected", + !mekf_init + ( + &filter, + identity, + zero_bias, + &config + ) + ); + +} /* test_mekf_init_rejects_invalid_uncertainty */ + +/** + * @brief Verifies rejection of invalid process-noise values. + */ +void test_mekf_init_rejects_invalid_process_noise + ( + void + ) +{ +MEKF_FILTER filter; +MEKF_CONFIG config; + +QUAT identity = { 1.0f, 0.0f, 0.0f, 0.0f }; +VECTOR_3F zero_bias = { 0.0f, 0.0f, 0.0f }; + +config = make_valid_config(); +config.gyro_noise_density_rad_s_sqrt_hz = -0.005f; + +TEST_ASSERT_TRUE + ( + "Negative gyro noise density is rejected", + !mekf_init + ( + &filter, + identity, + zero_bias, + &config + ) + ); + +config = make_valid_config(); +config.gyro_bias_random_walk_rad_s2_sqrt_hz = NAN; + +TEST_ASSERT_TRUE + ( + "Nonfinite bias random walk is rejected", + !mekf_init + ( + &filter, + identity, + zero_bias, + &config + ) + ); + +config = make_valid_config(); +config.gyro_noise_density_rad_s_sqrt_hz = FLT_MAX; + +TEST_ASSERT_TRUE + ( + "Gyro noise variance overflow is rejected", + !mekf_init + ( + &filter, + identity, + zero_bias, + &config + ) + ); + +} /* test_mekf_init_rejects_invalid_process_noise */ + +/** + * @brief Verifies rejection of invalid maximum timestep values. + */ +void test_mekf_init_rejects_invalid_maximum_timestep + ( + void + ) +{ +MEKF_FILTER filter; +MEKF_CONFIG config; + +QUAT identity = { 1.0f, 0.0f, 0.0f, 0.0f }; +VECTOR_3F zero_bias = { 0.0f, 0.0f, 0.0f }; + +config = make_valid_config(); +config.maximum_delta_time_s = 0.0f; + +TEST_ASSERT_TRUE + ( + "Zero maximum timestep is rejected", + !mekf_init + ( + &filter, + identity, + zero_bias, + &config + ) + ); + +config = make_valid_config(); +config.maximum_delta_time_s = -0.10f; + +TEST_ASSERT_TRUE + ( + "Negative maximum timestep is rejected", + !mekf_init + ( + &filter, + identity, + zero_bias, + &config + ) + ); + +config = make_valid_config(); +config.maximum_delta_time_s = NAN; + +TEST_ASSERT_TRUE + ( + "Nonfinite maximum timestep is rejected", + !mekf_init + ( + &filter, + identity, + zero_bias, + &config + ) + ); + +} /* test_mekf_init_rejects_invalid_maximum_timestep */ + +/** + * @brief Verifies that zero uncertainty and process noise are permitted. + * + * Zero values are mathematically valid and useful for deterministic unit tests, + * even though real flight configuration should use measured nonzero values. + */ +void test_mekf_init_accepts_zero_uncertainty_and_noise + ( + void + ) +{ +unsigned int row; +unsigned int column; + +MEKF_FILTER filter; +MEKF_CONFIG config = make_valid_config(); + +QUAT identity = { 1.0f, 0.0f, 0.0f, 0.0f }; +VECTOR_3F zero_bias = { 0.0f, 0.0f, 0.0f }; + +config.initial_attitude_std_rad = zero_bias; +config.initial_gyro_bias_std_rad_s = zero_bias; +config.gyro_noise_density_rad_s_sqrt_hz = 0.0f; +config.gyro_bias_random_walk_rad_s2_sqrt_hz = 0.0f; + +TEST_ASSERT_TRUE + ( + "Zero uncertainty and process noise are accepted", + mekf_init + ( + &filter, + identity, + zero_bias, + &config + ) + ); + +for ( row = 0U; row < MEKF_ERROR_STATE_DIM; row++ ) + { + for ( column = 0U; column < MEKF_ERROR_STATE_DIM; column++ ) + { + TEST_ASSERT_EQ_FLOAT + ( + "Zero uncertainty produces zero covariance", + filter.covariance[row][column], + 0.0f + ); + } + } + +} /* test_mekf_init_accepts_zero_uncertainty_and_noise */ + +/** + * @brief Verifies that failed initialization does not partially modify state. + */ +void test_mekf_init_failure_preserves_filter + ( + void + ) +{ +MEKF_FILTER filter = { 0 }; +MEKF_CONFIG invalid_config = make_valid_config(); + +QUAT initial_attitude = { 1.0f, 0.0f, 0.0f, 0.0f }; +VECTOR_3F initial_bias = { 0.0f, 0.0f, 0.0f }; + +QUAT sentinel_attitude = { 0.5f, 0.5f, 0.5f, 0.5f }; +VECTOR_3F sentinel_bias = { 1.0f, 2.0f, 3.0f }; + +filter.attitude = sentinel_attitude; +filter.gyro_bias_rad_s = sentinel_bias; +filter.covariance[0][0] = 123.0f; +filter.config.maximum_delta_time_s = 0.25f; + +invalid_config.maximum_delta_time_s = -0.10f; + +TEST_ASSERT_TRUE + ( + "Invalid configuration is rejected", + !mekf_init + ( + &filter, + initial_attitude, + initial_bias, + &invalid_config + ) + ); + +assert_quat_components + ( + "Failed initialization preserves attitude", + filter.attitude, + sentinel_attitude + ); + +assert_vector_components + ( + "Failed initialization preserves gyro bias", + filter.gyro_bias_rad_s, + sentinel_bias + ); + +TEST_ASSERT_EQ_FLOAT + ( + "Failed initialization preserves covariance", + filter.covariance[0][0], + 123.0f + ); + +TEST_ASSERT_EQ_FLOAT + ( + "Failed initialization preserves configuration", + filter.config.maximum_delta_time_s, + 0.25f + ); + +} /* test_mekf_init_failure_preserves_filter */ + +/*------------------------------------------------------------------------------ + Main + ------------------------------------------------------------------------------*/ + +int main + ( + void + ) +{ +unit_test tests[] = + { + { + "mekf_init_identity_and_bias", + test_mekf_init_identity_and_bias + }, + { + "mekf_init_normalizes_attitude", + test_mekf_init_normalizes_attitude + }, + { + "mekf_init_zero_quaternion_uses_identity", + test_mekf_init_zero_quaternion_uses_identity + }, + { + "mekf_init_sets_diagonal_covariance", + test_mekf_init_sets_diagonal_covariance + }, + { + "mekf_init_copies_config", + test_mekf_init_copies_config + }, + { + "mekf_init_rejects_null_pointers", + test_mekf_init_rejects_null_pointers + }, + { + "mekf_init_rejects_nonfinite_state", + test_mekf_init_rejects_nonfinite_state + }, + { + "mekf_init_rejects_invalid_uncertainty", + test_mekf_init_rejects_invalid_uncertainty + }, + { + "mekf_init_rejects_invalid_process_noise", + test_mekf_init_rejects_invalid_process_noise + }, + { + "mekf_init_rejects_invalid_maximum_timestep", + test_mekf_init_rejects_invalid_maximum_timestep + }, + { + "mekf_init_accepts_zero_uncertainty_and_noise", + test_mekf_init_accepts_zero_uncertainty_and_noise + }, + { + "mekf_init_failure_preserves_filter", + test_mekf_init_failure_preserves_filter + }, + }; + +TEST_INITIALIZE_TEST("mekf.c", tests); + +} /* main */ + +/******************************************************************************* + * END OF FILE + ******************************************************************************/ \ No newline at end of file From 33a2400dd3249a3863111e5a8f53269e44497b16 Mon Sep 17 00:00:00 2001 From: Bjorn Bengtsson Date: Tue, 28 Jul 2026 11:02:02 -0700 Subject: [PATCH 4/6] Add MEKF attitude and covariance prediction --- mekf/mekf.c | 474 +++++++++++++++++++++++++++++ mekf/mekf.h | 38 ++- test/mekf/test_mekf.c | 684 ++++++++++++++++++++++++++++++++++++++++++ 3 files changed, 1192 insertions(+), 4 deletions(-) diff --git a/mekf/mekf.c b/mekf/mekf.c index 60ddf29..7f2eb4c 100644 --- a/mekf/mekf.c +++ b/mekf/mekf.c @@ -6,6 +6,7 @@ * DESCRIPTION: * Multiplicative Extended Kalman Filter attitude estimator implementation. * + * ******************************************************************************/ /*------------------------------------------------------------------------------ @@ -19,6 +20,15 @@ ------------------------------------------------------------------------------*/ #include "mekf.h" +/*------------------------------------------------------------------------------ + Private Macros + ------------------------------------------------------------------------------*/ + +/** + * @brief Rotation magnitude below which the small-angle approximation is used. + */ +#define MEKF_SMALL_ANGLE_RAD 1.0e-6f + /*------------------------------------------------------------------------------ Private Functions ------------------------------------------------------------------------------*/ @@ -152,6 +162,34 @@ return true; } /* mekf_config_is_valid */ +/** + * @brief Determines whether every covariance entry is finite. + */ +static bool mekf_covariance_is_finite + ( + const float covariance + [MEKF_ERROR_STATE_DIM] + [MEKF_ERROR_STATE_DIM] + ) +{ +unsigned int row; +unsigned int column; + +for ( row = 0U; row < MEKF_ERROR_STATE_DIM; row++ ) + { + for ( column = 0U; column < MEKF_ERROR_STATE_DIM; column++ ) + { + if ( !isfinite(covariance[row][column]) ) + { + return false; + } + } + } + +return true; + +} /* mekf_covariance_is_finite */ + /*------------------------------------------------------------------------------ Public Functions ------------------------------------------------------------------------------*/ @@ -247,6 +285,442 @@ return true; } /* mekf_init */ +/* + * mekf_predict + * Predicts the next nominal attitude and error-state covariance using the + * current gyroscope measurement. + * + * Execution steps: + * + * 1. Take the measured angular velocity and correct it by subtracting the estimated bias: + * + * omega_corrected = omega_measured - bias_estimated + * + * 2. Integrate the corrected angular rate over the timestep to obtain the + * incremental body-frame rotation vector: + * + * delta_theta = omega_corrected * delta_time + * + * 3. Convert the rotation vector into an incremental rotation quaternion: + * + * delta_q = + * [ + * cos(norm(delta_theta) / 2), + * delta_theta / norm(delta_theta) * + * sin(norm(delta_theta) / 2) + * ] + * + * 4. Apply the incremental quaternion through right multiplication: + * + * q_new = normalize(q_old * delta_q) + * + * 5. Construct the error-state transition matrix and propagate covariance: + * + * P_new = Phi * P_old * transpose(Phi) + Q_d + * + * 6. Commit the predicted attitude and covariance only after both calculations + * complete successfully. + */ +bool mekf_predict + ( + MEKF_FILTER *filter, + VECTOR_3F gyro_body_rad_s, + float delta_time_s + ) +{ +unsigned int row; +unsigned int column; +unsigned int inner; +unsigned int axis; + +VECTOR_3F corrected_gyro_rad_s; +VECTOR_3F rotation_vector_rad; + +QUAT delta_attitude; +QUAT predicted_attitude; + +float state_transition + [MEKF_ERROR_STATE_DIM] + [MEKF_ERROR_STATE_DIM] = + { + { 0.0f } + }; + +float transition_covariance + [MEKF_ERROR_STATE_DIM] + [MEKF_ERROR_STATE_DIM]; + +float predicted_covariance + [MEKF_ERROR_STATE_DIM] + [MEKF_ERROR_STATE_DIM]; + +float rotation_magnitude_rad; +float half_rotation_rad; +float quaternion_vector_scale; + +float gyro_noise_variance; +float bias_random_walk_variance; + +float delta_time_squared; +float delta_time_cubed; + +float attitude_process_variance; +float attitude_bias_process_covariance; +float bias_process_variance; + +float matrix_sum; +float symmetric_value; + +if ( filter == NULL ) + { + return false; + } + +if ( !mekf_quat_is_finite(filter->attitude) ) + { + return false; + } + +if ( !mekf_vector_is_finite(filter->gyro_bias_rad_s) || + !mekf_vector_is_finite(gyro_body_rad_s) ) + { + return false; + } + +if ( !mekf_config_is_valid(&filter->config) ) + { + return false; + } + +if ( !mekf_covariance_is_finite(filter->covariance) ) + { + return false; + } + +if ( !isfinite(delta_time_s) || + delta_time_s <= 0.0f || + delta_time_s > filter->config.maximum_delta_time_s ) + { + return false; + } + +/* + * The raw gyro measurement (gyro_body_rad_s) consists of the true angular rate + * plus sensor bias and measurement noise: + * + * omega_m = omega_true + b_g + n_g + * + * Where: + * omega_m is what the gyroscope reports. + * omega_true is how fast the rocket is actually rotating. + * b_g is the true gyro bias, a slowly changing offset. + * n_g is random measurement noise. + * + * The true bias is unknown, so subtract the current nominal bias estimate, + * filter->gyro_bias_rad_s, to obtain the corrected angular rate: + * + * omega_corrected = omega_m - b_hat_g + * + * In the code: + * + * corrected_gyro_rad_s = + * gyro_body_rad_s - filter->gyro_bias_rad_s + */ +corrected_gyro_rad_s.x = + gyro_body_rad_s.x - filter->gyro_bias_rad_s.x; + +corrected_gyro_rad_s.y = + gyro_body_rad_s.y - filter->gyro_bias_rad_s.y; + +corrected_gyro_rad_s.z = + gyro_body_rad_s.z - filter->gyro_bias_rad_s.z; + +/* + * Integrate angular velocity over the timestep: + * + * delta_theta = omega_corrected * delta_time + * + * This produces the incremental body-frame rotation vector. Its magnitude is + * the total rotation angle: + * + * theta = + * norm(delta_theta) = + * sqrt + * ( + * delta_theta_x^2 + + * delta_theta_y^2 + + * delta_theta_z^2 + * ) + */ +rotation_vector_rad.x = corrected_gyro_rad_s.x * delta_time_s; +rotation_vector_rad.y = corrected_gyro_rad_s.y * delta_time_s; +rotation_vector_rad.z = corrected_gyro_rad_s.z * delta_time_s; + +rotation_magnitude_rad = sqrtf + ( + rotation_vector_rad.x * rotation_vector_rad.x + + rotation_vector_rad.y * rotation_vector_rad.y + + rotation_vector_rad.z * rotation_vector_rad.z + ); + +if ( !isfinite(rotation_magnitude_rad) ) + { + return false; + } + +/* + * Construct the incremental rotation quaternion: + * + * delta_q = + * [ + * cos(theta / 2), + * rotation_vector * sin(theta / 2) / theta + * ] + * + * For a very small rotation: + * + * sin(theta / 2) / theta approaches 0.5 + * + * The small-angle branch avoids division by a value close to zero. + */ +if ( rotation_magnitude_rad <= MEKF_SMALL_ANGLE_RAD ) + { + delta_attitude.w = 1.0f; + quaternion_vector_scale = 0.5f; + } +else + { + half_rotation_rad = 0.5f * rotation_magnitude_rad; + + delta_attitude.w = cosf(half_rotation_rad); + + quaternion_vector_scale = + sinf(half_rotation_rad) / + rotation_magnitude_rad; + } + +delta_attitude.x = + rotation_vector_rad.x * quaternion_vector_scale; + +delta_attitude.y = + rotation_vector_rad.y * quaternion_vector_scale; + +delta_attitude.z = + rotation_vector_rad.z * quaternion_vector_scale; + +/* + * The stored quaternion maps body coordinates into world coordinates, and the + * angular velocity is expressed in the body frame. Therefore, apply the + * incremental rotation through right multiplication: + * + * q_new = q_old * delta_q + */ +predicted_attitude = quat_mult + ( + filter->attitude, + delta_attitude + ); + +predicted_attitude = quat_normalize(predicted_attitude); + +/* + * This whole next section propagates the filter’s six-element error state + * + * Construct the first-order discrete error-state transition matrix: + * + * Phi = + * [ + * I - skew(omega) * dt -I * dt + * 0 I + * ] + * + * The upper-right block expresses that an error in the gyro-bias estimate + * produces an error in propagated attitude. + * An error that was entirely about one body axis can appear + * partly along another axis after rotation. + * + * Constructs: + * -[ω]× * Δt = [ 0 -ω_z*Δt ω_y*Δt ] + * [ ω_z*Δt 0 -ω_x*Δt ] + * [ -ω_y*Δt ω_x*Δt 0 ] + */ +for ( row = 0U; row < MEKF_ERROR_STATE_DIM; row++ ) + { + state_transition[row][row] = 1.0f; + } + +state_transition + [MEKF_ATTITUDE_ERROR_X] + [MEKF_ATTITUDE_ERROR_Y] = + corrected_gyro_rad_s.z * delta_time_s; + +state_transition + [MEKF_ATTITUDE_ERROR_X] + [MEKF_ATTITUDE_ERROR_Z] = + -corrected_gyro_rad_s.y * delta_time_s; + +state_transition + [MEKF_ATTITUDE_ERROR_Y] + [MEKF_ATTITUDE_ERROR_X] = + -corrected_gyro_rad_s.z * delta_time_s; + +state_transition + [MEKF_ATTITUDE_ERROR_Y] + [MEKF_ATTITUDE_ERROR_Z] = + corrected_gyro_rad_s.x * delta_time_s; + +state_transition + [MEKF_ATTITUDE_ERROR_Z] + [MEKF_ATTITUDE_ERROR_X] = + corrected_gyro_rad_s.y * delta_time_s; + +state_transition + [MEKF_ATTITUDE_ERROR_Z] + [MEKF_ATTITUDE_ERROR_Y] = + -corrected_gyro_rad_s.x * delta_time_s; + +for ( axis = 0U; axis < 3U; axis++ ) + { + state_transition[axis][axis + 3U] = -delta_time_s; + } + +/* + * First multiplication: + * + * transition_covariance = Phi * P + */ +for ( row = 0U; row < MEKF_ERROR_STATE_DIM; row++ ) + { + for ( column = 0U; column < MEKF_ERROR_STATE_DIM; column++ ) + { + matrix_sum = 0.0f; + + for ( inner = 0U; inner < MEKF_ERROR_STATE_DIM; inner++ ) + { + matrix_sum += + state_transition[row][inner] * + filter->covariance[inner][column]; + } + + transition_covariance[row][column] = matrix_sum; + } + } + +/* + * Second multiplication: + * + * predicted_covariance = + * transition_covariance * transpose(Phi) + */ +for ( row = 0U; row < MEKF_ERROR_STATE_DIM; row++ ) + { + for ( column = 0U; column < MEKF_ERROR_STATE_DIM; column++ ) + { + matrix_sum = 0.0f; + + for ( inner = 0U; inner < MEKF_ERROR_STATE_DIM; inner++ ) + { + matrix_sum += + transition_covariance[row][inner] * + state_transition[column][inner]; + } + + predicted_covariance[row][column] = matrix_sum; + } + } + +/* + * Construct the discrete process-noise contribution. Gyro white noise directly + * increases attitude uncertainty. Bias random walk increases bias uncertainty + * and also contributes to attitude and attitude-bias uncertainty during the + * timestep. + */ +gyro_noise_variance = + filter->config.gyro_noise_density_rad_s_sqrt_hz * + filter->config.gyro_noise_density_rad_s_sqrt_hz; + +bias_random_walk_variance = + filter->config.gyro_bias_random_walk_rad_s2_sqrt_hz * + filter->config.gyro_bias_random_walk_rad_s2_sqrt_hz; + +delta_time_squared = delta_time_s * delta_time_s; +delta_time_cubed = delta_time_squared * delta_time_s; + +attitude_process_variance = + gyro_noise_variance * delta_time_s + + bias_random_walk_variance * + delta_time_cubed / + 3.0f; + +attitude_bias_process_covariance = + -bias_random_walk_variance * + delta_time_squared / + 2.0f; + +bias_process_variance = + bias_random_walk_variance * delta_time_s; + +for ( axis = 0U; axis < 3U; axis++ ) + { + predicted_covariance[axis][axis] += + attitude_process_variance; + + predicted_covariance[axis][axis + 3U] += + attitude_bias_process_covariance; + + predicted_covariance[axis + 3U][axis] += + attitude_bias_process_covariance; + + predicted_covariance[axis + 3U][axis + 3U] += + bias_process_variance; + } + +/* + * Floating-point matrix multiplication can introduce very small asymmetric + * roundoff. Explicitly restore covariance symmetry. + */ +for ( row = 0U; row < MEKF_ERROR_STATE_DIM; row++ ) + { + for ( column = row + 1U; + column < MEKF_ERROR_STATE_DIM; + column++ ) + { + symmetric_value = + 0.5f * + ( + predicted_covariance[row][column] + + predicted_covariance[column][row] + ); + + predicted_covariance[row][column] = symmetric_value; + predicted_covariance[column][row] = symmetric_value; + } + } + +if ( !mekf_covariance_is_finite(predicted_covariance) ) + { + return false; + } + +/* + * Commit the nominal attitude and covariance only after both predictions have + * completed successfully. + */ +filter->attitude = predicted_attitude; + +for ( row = 0U; row < MEKF_ERROR_STATE_DIM; row++ ) + { + for ( column = 0U; column < MEKF_ERROR_STATE_DIM; column++ ) + { + filter->covariance[row][column] = + predicted_covariance[row][column]; + } + } + +return true; + +} /* mekf_predict */ + /******************************************************************************* * END OF FILE ******************************************************************************/ \ No newline at end of file diff --git a/mekf/mekf.h b/mekf/mekf.h index 9f54705..9421cd0 100644 --- a/mekf/mekf.h +++ b/mekf/mekf.h @@ -73,29 +73,59 @@ typedef enum _MEKF_ERROR_STATE_INDEX typedef struct _MEKF_CONFIG { /** - * Initial one-sigma local attitude uncertainty in radians. - */ + * Initial one-sigma uncertainty of the local attitude-error estimate, in radians. + * + * Each component specifies the standard deviation of the unknown small-angle + * rotation between the nominal attitude estimate and the true attitude about + * the body X, Y, and Z axes. + * + * These values describe uncertainty in the attitude-error estimate; they are + * not estimates of the attitude error itself. The expected initial attitude + * error is zero. mekf_init() squares these standard deviations to initialize + * the corresponding attitude-error covariance diagonal entries. + * + * A larger value tells the filter that the initial attitude estimate is less + * trustworthy, while a smaller value indicates greater confidence in it. + */ VECTOR_3F initial_attitude_std_rad; /** - * Initial one-sigma gyro-bias uncertainty in radians per second. + * Initial one-sigma uncertainty of the gyro-bias estimate, in radians per second. + * + * Each component specifies the standard deviation of the unknown difference + * between the estimated gyro bias and the true gyro bias about the body X, Y, and Z axes. + * + * These values describe uncertainty in the bias estimate; they are not the + * estimated gyro-bias values themselves. The expected initial gyro-bias error + * is zero. mekf_init() squares these standard deviations to initialize the + * corresponding gyro-bias covariance diagonal entries. + * + * A larger value tells the filter that the initial gyro-bias estimate is less + * trustworthy, while a smaller value indicates greater confidence in it. */ VECTOR_3F initial_gyro_bias_std_rad_s; /** * Continuous gyroscope white-noise density in radians per second per - * square-root hertz. + * square-root hertz. This represents short term random noise in the gyro measurement. + * A noisier gyro will produce a larger attitude covariance growth during prediction. */ float gyro_noise_density_rad_s_sqrt_hz; /** * Continuous gyro-bias random-walk density in radians per second squared * per square-root hertz. + * This represents how quickly the gyro bias is expected to drift over time. + * A larger random-walk density will produce a larger gyro-bias covariance growth during prediction. + * Bias can change due to temperature, sensor warmup, mechanical stress, etc.. */ float gyro_bias_random_walk_rad_s2_sqrt_hz; /** * Maximum valid gyro prediction timestep in seconds. + * This represents the largest allowable time interval for one prediction. + * This protects the estimator from propagating across unreasonable timing gaps + * caused by missed data, timestamp corruption, or task delays. */ float maximum_delta_time_s; diff --git a/test/mekf/test_mekf.c b/test/mekf/test_mekf.c index a294108..4a691f5 100644 --- a/test/mekf/test_mekf.c +++ b/test/mekf/test_mekf.c @@ -763,6 +763,662 @@ TEST_ASSERT_EQ_FLOAT } /* test_mekf_init_failure_preserves_filter */ +/*------------------------------------------------------------------------------ + Gyro Prediction Tests + ------------------------------------------------------------------------------*/ + +/** + * @brief Verifies that zero angular rate preserves nominal attitude. + */ +void test_mekf_predict_zero_rate + ( + void + ) +{ +MEKF_FILTER filter; +MEKF_CONFIG config = make_valid_config(); + +QUAT identity = { 1.0f, 0.0f, 0.0f, 0.0f }; +VECTOR_3F zero_vector = { 0.0f, 0.0f, 0.0f }; + +TEST_ASSERT_TRUE + ( + "MEKF initialization succeeds", + mekf_init + ( + &filter, + identity, + zero_vector, + &config + ) + ); + +TEST_ASSERT_TRUE + ( + "Zero-rate prediction succeeds", + mekf_predict + ( + &filter, + zero_vector, + 0.01f + ) + ); + +assert_quat_components + ( + "Zero angular rate preserves attitude", + filter.attitude, + identity + ); + +assert_vector_components + ( + "Prediction preserves gyro-bias estimate", + filter.gyro_bias_rad_s, + zero_vector + ); + +} /* test_mekf_predict_zero_rate */ + +/** + * @brief Verifies positive rotation about body Z for one second. + * + * A positive 90-degree-per-second body-Z rate should rotate body +X toward + * world +Y after one second. + */ +void test_mekf_predict_positive_yaw + ( + void + ) +{ +unsigned int step; + +MEKF_FILTER filter; +MEKF_CONFIG config = make_valid_config(); + +QUAT identity = { 1.0f, 0.0f, 0.0f, 0.0f }; + +QUAT expected = + { + .w = 0.70710678f, + .x = 0.0f, + .y = 0.0f, + .z = 0.70710678f + }; + +VECTOR_3F zero_bias = { 0.0f, 0.0f, 0.0f }; + +VECTOR_3F gyro_body_rad_s = + { + .x = 0.0f, + .y = 0.0f, + .z = deg_to_rad(90.0f) + }; + +TEST_ASSERT_TRUE + ( + "MEKF initialization succeeds", + mekf_init + ( + &filter, + identity, + zero_bias, + &config + ) + ); + +for ( step = 0U; step < 100U; step++ ) + { + TEST_ASSERT_TRUE + ( + "Positive-yaw prediction succeeds", + mekf_predict + ( + &filter, + gyro_body_rad_s, + 0.01f + ) + ); + } + +assert_quat_components + ( + "Positive body-Z rate produces positive yaw", + filter.attitude, + expected + ); + +} /* test_mekf_predict_positive_yaw */ + +/** + * @brief Verifies that the nominal gyro bias is subtracted. + */ +void test_mekf_predict_subtracts_gyro_bias + ( + void + ) +{ +unsigned int step; + +MEKF_FILTER filter; +MEKF_CONFIG config = make_valid_config(); + +QUAT identity = { 1.0f, 0.0f, 0.0f, 0.0f }; + +VECTOR_3F initial_bias = + { + .x = 0.0f, + .y = 0.0f, + .z = deg_to_rad(10.0f) + }; + +/* + * The measured rate exactly equals the estimated bias, so corrected angular + * velocity should be zero. + */ +VECTOR_3F gyro_measurement = initial_bias; + +TEST_ASSERT_TRUE + ( + "MEKF initialization succeeds", + mekf_init + ( + &filter, + identity, + initial_bias, + &config + ) + ); + +for ( step = 0U; step < 100U; step++ ) + { + TEST_ASSERT_TRUE + ( + "Bias-corrected prediction succeeds", + mekf_predict + ( + &filter, + gyro_measurement, + 0.01f + ) + ); + } + +assert_quat_components + ( + "Measured rate equal to bias produces no rotation", + filter.attitude, + identity + ); + +assert_vector_components + ( + "Prediction does not alter nominal bias", + filter.gyro_bias_rad_s, + initial_bias + ); + +} /* test_mekf_predict_subtracts_gyro_bias */ + +/*------------------------------------------------------------------------------ + Covariance Prediction Tests + ------------------------------------------------------------------------------*/ + +/** + * @brief Verifies that gyro-bias uncertainty propagates into attitude + * uncertainty. + * + * With zero angular rate and zero process noise: + * + * Phi = + * [ + * I -I * dt + * 0 I + * ] + * + * An initial bias variance of 4.0 and timestep of 0.1 seconds should produce: + * + * attitude variance = 1.0 + 4.0 * 0.1^2 = 1.04 + * attitude-bias covariance = -4.0 * 0.1 = -0.4 + * bias variance = 4.0 + */ +void test_mekf_predict_couples_bias_uncertainty + ( + void + ) +{ +unsigned int axis; +unsigned int row; +unsigned int column; + +MEKF_FILTER filter; +MEKF_CONFIG config = make_valid_config(); + +float expected[MEKF_ERROR_STATE_DIM][MEKF_ERROR_STATE_DIM] = + { + { 0.0f } + }; + +QUAT identity = { 1.0f, 0.0f, 0.0f, 0.0f }; +VECTOR_3F zero_vector = { 0.0f, 0.0f, 0.0f }; + +config.initial_attitude_std_rad.x = 1.0f; +config.initial_attitude_std_rad.y = 1.0f; +config.initial_attitude_std_rad.z = 1.0f; + +config.initial_gyro_bias_std_rad_s.x = 2.0f; +config.initial_gyro_bias_std_rad_s.y = 2.0f; +config.initial_gyro_bias_std_rad_s.z = 2.0f; + +config.gyro_noise_density_rad_s_sqrt_hz = 0.0f; +config.gyro_bias_random_walk_rad_s2_sqrt_hz = 0.0f; +config.maximum_delta_time_s = 0.10f; + +TEST_ASSERT_TRUE + ( + "MEKF initialization succeeds", + mekf_init + ( + &filter, + identity, + zero_vector, + &config + ) + ); + +TEST_ASSERT_TRUE + ( + "Zero-rate covariance prediction succeeds", + mekf_predict + ( + &filter, + zero_vector, + 0.10f + ) + ); + +for ( axis = 0U; axis < 3U; axis++ ) + { + expected[axis][axis] = 1.04f; + expected[axis + 3U][axis + 3U] = 4.0f; + + expected[axis][axis + 3U] = -0.40f; + expected[axis + 3U][axis] = -0.40f; + } + +for ( row = 0U; row < MEKF_ERROR_STATE_DIM; row++ ) + { + for ( column = 0U; column < MEKF_ERROR_STATE_DIM; column++ ) + { + TEST_ASSERT_EQ_FLOAT + ( + "Predicted covariance entry", + filter.covariance[row][column], + expected[row][column] + ); + } + } + +} /* test_mekf_predict_couples_bias_uncertainty */ + +/** + * @brief Verifies discrete gyro and gyro-bias process-noise propagation. + * + * This test starts with zero covariance so the predicted covariance consists + * entirely of the discrete process-noise matrix Q_d. + */ +void test_mekf_predict_adds_process_noise + ( + void + ) +{ +unsigned int axis; +unsigned int row; +unsigned int column; + +MEKF_FILTER filter; +MEKF_CONFIG config = make_valid_config(); + +float expected[MEKF_ERROR_STATE_DIM][MEKF_ERROR_STATE_DIM] = + { + { 0.0f } + }; + +QUAT identity = { 1.0f, 0.0f, 0.0f, 0.0f }; +VECTOR_3F zero_vector = { 0.0f, 0.0f, 0.0f }; + +float expected_attitude_variance; +float expected_attitude_bias_covariance; +float expected_bias_variance; + +/* + * Start with zero covariance so only Q_d contributes to the result. + */ +config.initial_attitude_std_rad = zero_vector; +config.initial_gyro_bias_std_rad_s = zero_vector; + +/* + * Use intentionally large synthetic noise values so every expected process + * noise term is easily distinguishable from zero in the unit test. + */ +config.gyro_noise_density_rad_s_sqrt_hz = 2.0f; +config.gyro_bias_random_walk_rad_s2_sqrt_hz = 1.0f; +config.maximum_delta_time_s = 0.10f; + +/* + * For dt = 0.1 seconds: + * + * sigma_g^2 = 4 + * sigma_b^2 = 1 + * + * Q_theta_theta = + * sigma_g^2 * dt + sigma_b^2 * dt^3 / 3 + * + * Q_theta_bias = + * -sigma_b^2 * dt^2 / 2 + * + * Q_bias_bias = + * sigma_b^2 * dt + */ +expected_attitude_variance = + 4.0f * 0.10f + + 1.0f * 0.001f / 3.0f; + +expected_attitude_bias_covariance = + -1.0f * 0.01f / 2.0f; + +expected_bias_variance = + 1.0f * 0.10f; + +TEST_ASSERT_TRUE + ( + "MEKF initialization succeeds", + mekf_init + ( + &filter, + identity, + zero_vector, + &config + ) + ); + +TEST_ASSERT_TRUE + ( + "Process-noise prediction succeeds", + mekf_predict + ( + &filter, + zero_vector, + 0.10f + ) + ); + +for ( axis = 0U; axis < 3U; axis++ ) + { + expected[axis][axis] = + expected_attitude_variance; + + expected[axis][axis + 3U] = + expected_attitude_bias_covariance; + + expected[axis + 3U][axis] = + expected_attitude_bias_covariance; + + expected[axis + 3U][axis + 3U] = + expected_bias_variance; + } + +for ( row = 0U; row < MEKF_ERROR_STATE_DIM; row++ ) + { + for ( column = 0U; column < MEKF_ERROR_STATE_DIM; column++ ) + { + TEST_ASSERT_EQ_FLOAT + ( + "Discrete process-noise entry", + filter.covariance[row][column], + expected[row][column] + ); + } + } + +} /* test_mekf_predict_adds_process_noise */ + +/** + * @brief Verifies that invalid prediction timesteps are rejected without + * modifying the attitude or covariance. + */ +void test_mekf_predict_rejects_invalid_timestep + ( + void + ) +{ +unsigned int row; +unsigned int column; + +MEKF_FILTER filter; +MEKF_FILTER original_filter; + +MEKF_CONFIG config = make_valid_config(); + +QUAT identity = { 1.0f, 0.0f, 0.0f, 0.0f }; +VECTOR_3F zero_bias = { 0.0f, 0.0f, 0.0f }; +VECTOR_3F gyro_body_rad_s = { 0.1f, -0.2f, 0.3f }; + +TEST_ASSERT_TRUE + ( + "MEKF initialization succeeds", + mekf_init + ( + &filter, + identity, + zero_bias, + &config + ) + ); + +original_filter = filter; + +TEST_ASSERT_FALSE + ( + "Zero timestep is rejected", + mekf_predict + ( + &filter, + gyro_body_rad_s, + 0.0f + ) + ); + +TEST_ASSERT_FALSE + ( + "Negative timestep is rejected", + mekf_predict + ( + &filter, + gyro_body_rad_s, + -0.01f + ) + ); + +TEST_ASSERT_FALSE + ( + "Timestep above configured maximum is rejected", + mekf_predict + ( + &filter, + gyro_body_rad_s, + config.maximum_delta_time_s + 0.01f + ) + ); + +TEST_ASSERT_FALSE + ( + "Nonfinite timestep is rejected", + mekf_predict + ( + &filter, + gyro_body_rad_s, + NAN + ) + ); + +TEST_ASSERT_EQ_FLOAT + ( + "Attitude W remains unchanged", + filter.attitude.w, + original_filter.attitude.w + ); + +TEST_ASSERT_EQ_FLOAT + ( + "Attitude X remains unchanged", + filter.attitude.x, + original_filter.attitude.x + ); + +TEST_ASSERT_EQ_FLOAT + ( + "Attitude Y remains unchanged", + filter.attitude.y, + original_filter.attitude.y + ); + +TEST_ASSERT_EQ_FLOAT + ( + "Attitude Z remains unchanged", + filter.attitude.z, + original_filter.attitude.z + ); + +for ( row = 0U; row < MEKF_ERROR_STATE_DIM; row++ ) + { + for ( column = 0U; column < MEKF_ERROR_STATE_DIM; column++ ) + { + TEST_ASSERT_EQ_FLOAT + ( + "Covariance remains unchanged", + filter.covariance[row][column], + original_filter.covariance[row][column] + ); + } + } + +} /* test_mekf_predict_rejects_invalid_timestep */ + +/** + * @brief Verifies attitude-covariance propagation during nonzero rotation. + * + * For a corrected Z-axis rate of 1 rad/s and dt = 0.1 s, the attitude + * transition block is: + * + * Phi_theta = + * [ + * 1.0 0.1 0.0 + * -0.1 1.0 0.0 + * 0.0 0.0 1.0 + * ] + * + * Starting with attitude covariance diag(1, 4, 9), the propagated attitude + * covariance should be: + * + * [ + * 1.04 0.30 0.00 + * 0.30 4.01 0.00 + * 0.00 0.00 9.00 + * ] + */ +void test_mekf_predict_rotates_attitude_covariance + ( + void + ) +{ +unsigned int row; +unsigned int column; + +MEKF_FILTER filter; +MEKF_CONFIG config = make_valid_config(); + +float expected[MEKF_ERROR_STATE_DIM][MEKF_ERROR_STATE_DIM] = + { + { 0.0f } + }; + +QUAT identity = { 1.0f, 0.0f, 0.0f, 0.0f }; + +VECTOR_3F zero_vector = + { + 0.0f, + 0.0f, + 0.0f + }; + +VECTOR_3F gyro_body_rad_s = + { + 0.0f, + 0.0f, + 1.0f + }; + +/* + * Use unequal initial attitude variances so an incorrect skew-matrix sign or + * axis placement cannot accidentally produce the expected result. + */ +config.initial_attitude_std_rad.x = 1.0f; +config.initial_attitude_std_rad.y = 2.0f; +config.initial_attitude_std_rad.z = 3.0f; + +config.initial_gyro_bias_std_rad_s = zero_vector; + +config.gyro_noise_density_rad_s_sqrt_hz = 0.0f; +config.gyro_bias_random_walk_rad_s2_sqrt_hz = 0.0f; +config.maximum_delta_time_s = 0.10f; + +TEST_ASSERT_TRUE + ( + "MEKF initialization succeeds", + mekf_init + ( + &filter, + identity, + zero_vector, + &config + ) + ); + +TEST_ASSERT_TRUE + ( + "Rotating covariance prediction succeeds", + mekf_predict + ( + &filter, + gyro_body_rad_s, + 0.10f + ) + ); + +expected[MEKF_ATTITUDE_ERROR_X][MEKF_ATTITUDE_ERROR_X] = 1.04f; +expected[MEKF_ATTITUDE_ERROR_X][MEKF_ATTITUDE_ERROR_Y] = 0.30f; + +expected[MEKF_ATTITUDE_ERROR_Y][MEKF_ATTITUDE_ERROR_X] = 0.30f; +expected[MEKF_ATTITUDE_ERROR_Y][MEKF_ATTITUDE_ERROR_Y] = 4.01f; + +expected[MEKF_ATTITUDE_ERROR_Z][MEKF_ATTITUDE_ERROR_Z] = 9.00f; + +for ( row = 0U; row < MEKF_ERROR_STATE_DIM; row++ ) + { + for ( column = 0U; column < MEKF_ERROR_STATE_DIM; column++ ) + { + TEST_ASSERT_EQ_FLOAT + ( + "Rotating attitude covariance entry", + filter.covariance[row][column], + expected[row][column] + ); + } + } + +} /* test_mekf_predict_rotates_attitude_covariance */ + /*------------------------------------------------------------------------------ Main ------------------------------------------------------------------------------*/ @@ -822,6 +1478,34 @@ unit_test tests[] = "mekf_init_failure_preserves_filter", test_mekf_init_failure_preserves_filter }, + { + "mekf_predict_zero_rate", + test_mekf_predict_zero_rate + }, + { + "mekf_predict_positive_yaw", + test_mekf_predict_positive_yaw + }, + { + "mekf_predict_subtracts_gyro_bias", + test_mekf_predict_subtracts_gyro_bias + }, + { + "mekf_predict_couples_bias_uncertainty", + test_mekf_predict_couples_bias_uncertainty + }, + { + "mekf_predict_adds_process_noise", + test_mekf_predict_adds_process_noise + }, + { + "mekf_predict_rejects_invalid_timestep", + test_mekf_predict_rejects_invalid_timestep + }, + { + "mekf_predict_rotates_attitude_covariance", + test_mekf_predict_rotates_attitude_covariance + } }; TEST_INITIALIZE_TEST("mekf.c", tests); From 90568711b5e24a85b5b6f8b60b9ba3c4b2c5416d Mon Sep 17 00:00:00 2001 From: Bjorn Bengtsson Date: Tue, 28 Jul 2026 20:15:15 -0700 Subject: [PATCH 5/6] Add MEKF accelerometer correction --- mekf/mekf.c | 746 +++++++++++++++++++++++++++++++++++ mekf/mekf.h | 66 +++- test/mekf/test_mekf.c | 893 +++++++++++++++++++++++++++++++++++++++++- 3 files changed, 1687 insertions(+), 18 deletions(-) diff --git a/mekf/mekf.c b/mekf/mekf.c index 7f2eb4c..7422552 100644 --- a/mekf/mekf.c +++ b/mekf/mekf.c @@ -29,6 +29,11 @@ */ #define MEKF_SMALL_ANGLE_RAD 1.0e-6f +/** + * @brief Smallest usable absolute determinant for a three-by-three matrix. + */ +#define MEKF_MATRIX_INVERSE_MIN_DETERMINANT 1.0e-20f + /*------------------------------------------------------------------------------ Private Functions ------------------------------------------------------------------------------*/ @@ -152,6 +157,30 @@ if ( !mekf_standard_deviation_is_valid return false; } +if ( !isfinite(config->accelerometer_direction_std) || + config->accelerometer_direction_std <= 0.0f ) + { + return false; + } + +if ( !isfinite(config->gravity_magnitude_m_s2) || + config->gravity_magnitude_m_s2 <= 0.0f ) + { + return false; + } + +if ( !isfinite(config->accelerometer_magnitude_tolerance_m_s2) || + config->accelerometer_magnitude_tolerance_m_s2 <= 0.0f ) + { + return false; + } + +if ( !isfinite(config->accelerometer_innovation_gate) || + config->accelerometer_innovation_gate <= 0.0f ) + { + return false; + } + if ( !isfinite(config->maximum_delta_time_s) || config->maximum_delta_time_s <= 0.0f ) { @@ -190,6 +219,112 @@ return true; } /* mekf_covariance_is_finite */ +/** + * @brief Inverts a finite nonsingular three-by-three matrix. + */ +static bool mekf_matrix_3x3_inverse + ( + const float matrix[3][3], + float inverse[3][3] + ) +{ +unsigned int row; +unsigned int column; + +float determinant; + +determinant = + matrix[0][0] * + ( + matrix[1][1] * matrix[2][2] - + matrix[1][2] * matrix[2][1] + ) - + matrix[0][1] * + ( + matrix[1][0] * matrix[2][2] - + matrix[1][2] * matrix[2][0] + ) + + matrix[0][2] * + ( + matrix[1][0] * matrix[2][1] - + matrix[1][1] * matrix[2][0] + ); + +if ( !isfinite(determinant) || + fabsf(determinant) <= MEKF_MATRIX_INVERSE_MIN_DETERMINANT ) + { + return false; + } + +inverse[0][0] = + ( + matrix[1][1] * matrix[2][2] - + matrix[1][2] * matrix[2][1] + ) / determinant; + +inverse[0][1] = + ( + matrix[0][2] * matrix[2][1] - + matrix[0][1] * matrix[2][2] + ) / determinant; + +inverse[0][2] = + ( + matrix[0][1] * matrix[1][2] - + matrix[0][2] * matrix[1][1] + ) / determinant; + +inverse[1][0] = + ( + matrix[1][2] * matrix[2][0] - + matrix[1][0] * matrix[2][2] + ) / determinant; + +inverse[1][1] = + ( + matrix[0][0] * matrix[2][2] - + matrix[0][2] * matrix[2][0] + ) / determinant; + +inverse[1][2] = + ( + matrix[0][2] * matrix[1][0] - + matrix[0][0] * matrix[1][2] + ) / determinant; + +inverse[2][0] = + ( + matrix[1][0] * matrix[2][1] - + matrix[1][1] * matrix[2][0] + ) / determinant; + +inverse[2][1] = + ( + matrix[0][1] * matrix[2][0] - + matrix[0][0] * matrix[2][1] + ) / determinant; + +inverse[2][2] = + ( + matrix[0][0] * matrix[1][1] - + matrix[0][1] * matrix[1][0] + ) / determinant; + +for ( row = 0U; row < 3U; row++ ) + { + for ( column = 0U; column < 3U; column++ ) + { + if ( !isfinite(inverse[row][column]) ) + { + return false; + } + } + } + +return true; + +} /* mekf_matrix_3x3_inverse */ + /*------------------------------------------------------------------------------ Public Functions ------------------------------------------------------------------------------*/ @@ -721,6 +856,617 @@ return true; } /* mekf_predict */ +bool mekf_update_accelerometer + ( + MEKF_FILTER *filter, + VECTOR_3F acceleration_body_m_s2 + ) +{ +unsigned int row; +unsigned int column; +unsigned int inner; +unsigned int measurement; + +VECTOR_3F measured_direction; +VECTOR_3F predicted_direction; +VECTOR_3F corrected_bias; + +QUAT gravity_world = { 0.0f, 0.0f, 0.0f, 1.0f }; +QUAT gravity_body; +QUAT correction_quaternion; +QUAT corrected_attitude; + +float measurement_jacobian[3][MEKF_ERROR_STATE_DIM] = + { + { 0.0f } + }; + +float covariance_measurement_cross[MEKF_ERROR_STATE_DIM][3]; +float innovation_covariance[3][3]; +float inverse_innovation_covariance[3][3]; +float kalman_gain[MEKF_ERROR_STATE_DIM][3]; + +float identity_minus_gain_jacobian + [MEKF_ERROR_STATE_DIM] + [MEKF_ERROR_STATE_DIM] = + { + { 0.0f } + }; + +float intermediate_covariance + [MEKF_ERROR_STATE_DIM] + [MEKF_ERROR_STATE_DIM]; + +float joseph_covariance + [MEKF_ERROR_STATE_DIM] + [MEKF_ERROR_STATE_DIM]; + +float reset_jacobian + [MEKF_ERROR_STATE_DIM] + [MEKF_ERROR_STATE_DIM] = + { + { 0.0f } + }; + +float reset_intermediate_covariance + [MEKF_ERROR_STATE_DIM] + [MEKF_ERROR_STATE_DIM]; + +float corrected_covariance + [MEKF_ERROR_STATE_DIM] + [MEKF_ERROR_STATE_DIM]; + +float residual[3]; +float error_state[MEKF_ERROR_STATE_DIM] = { 0.0f }; + +float acceleration_magnitude; +float predicted_direction_magnitude; +float measurement_variance; +float normalized_innovation_squared; +float correction_magnitude; +float half_correction; +float quaternion_vector_scale; +float matrix_sum; +float symmetric_value; + +if ( filter == NULL ) + { + return false; + } + +if ( !mekf_quat_is_finite(filter->attitude) || + !mekf_vector_is_finite(filter->gyro_bias_rad_s) || + !mekf_vector_is_finite(acceleration_body_m_s2) ) + { + return false; + } + +if ( !mekf_config_is_valid(&filter->config) || + !mekf_covariance_is_finite(filter->covariance) ) + { + return false; + } + +/* + * The accelerometer can be treated as a gravity reference only when its + * magnitude is sufficiently close to the expected gravity magnitude. + */ +acceleration_magnitude = sqrtf + ( + acceleration_body_m_s2.x * acceleration_body_m_s2.x + + acceleration_body_m_s2.y * acceleration_body_m_s2.y + + acceleration_body_m_s2.z * acceleration_body_m_s2.z + ); + +if ( !isfinite(acceleration_magnitude) || + acceleration_magnitude <= 0.0f ) + { + return false; + } + +if ( fabsf + ( + acceleration_magnitude - + filter->config.gravity_magnitude_m_s2 + ) > + filter->config.accelerometer_magnitude_tolerance_m_s2 ) + { + return false; + } + +measured_direction.x = + acceleration_body_m_s2.x / acceleration_magnitude; + +measured_direction.y = + acceleration_body_m_s2.y / acceleration_magnitude; + +measured_direction.z = + acceleration_body_m_s2.z / acceleration_magnitude; + +/* + * The stored attitude maps body coordinates into world coordinates. Rotate the + * world-frame unit gravity vector into the body frame to predict what the + * accelerometer direction should be. + */ +gravity_body = quat_rotate_world_to_body + ( + filter->attitude, + gravity_world + ); + +predicted_direction_magnitude = sqrtf + ( + gravity_body.x * gravity_body.x + + gravity_body.y * gravity_body.y + + gravity_body.z * gravity_body.z + ); + +if ( !isfinite(predicted_direction_magnitude) || + predicted_direction_magnitude <= 0.0f ) + { + return false; + } + +predicted_direction.x = + gravity_body.x / predicted_direction_magnitude; + +predicted_direction.y = + gravity_body.y / predicted_direction_magnitude; + +predicted_direction.z = + gravity_body.z / predicted_direction_magnitude; + +/* + * Measurement residual: + * + * residual = + * measured_gravity_direction - + * predicted_gravity_direction + */ +residual[0] = measured_direction.x - predicted_direction.x; +residual[1] = measured_direction.y - predicted_direction.y; +residual[2] = measured_direction.z - predicted_direction.z; + +/* + * For a right-multiplicative local attitude error, the linearized gravity + * measurement Jacobian is: + * + * H = [ skew(predicted_gravity_direction) 0 ] + */ +measurement_jacobian[0][MEKF_ATTITUDE_ERROR_Y] = + -predicted_direction.z; + +measurement_jacobian[0][MEKF_ATTITUDE_ERROR_Z] = + predicted_direction.y; + +measurement_jacobian[1][MEKF_ATTITUDE_ERROR_X] = + predicted_direction.z; + +measurement_jacobian[1][MEKF_ATTITUDE_ERROR_Z] = + -predicted_direction.x; + +measurement_jacobian[2][MEKF_ATTITUDE_ERROR_X] = + -predicted_direction.y; + +measurement_jacobian[2][MEKF_ATTITUDE_ERROR_Y] = + predicted_direction.x; + +/* + * Compute the state-to-measurement cross covariance: + * + * PHT = P * transpose(H) + */ +for ( row = 0U; row < MEKF_ERROR_STATE_DIM; row++ ) + { + for ( measurement = 0U; measurement < 3U; measurement++ ) + { + matrix_sum = 0.0f; + + for ( inner = 0U; inner < MEKF_ERROR_STATE_DIM; inner++ ) + { + matrix_sum += + filter->covariance[row][inner] * + measurement_jacobian[measurement][inner]; + } + + covariance_measurement_cross[row][measurement] = + matrix_sum; + } + } + +/* + * Innovation covariance: + * + * S = H * P * transpose(H) + R + */ +measurement_variance = + filter->config.accelerometer_direction_std * + filter->config.accelerometer_direction_std; + +for ( row = 0U; row < 3U; row++ ) + { + for ( column = 0U; column < 3U; column++ ) + { + matrix_sum = 0.0f; + + for ( inner = 0U; inner < MEKF_ERROR_STATE_DIM; inner++ ) + { + matrix_sum += + measurement_jacobian[row][inner] * + covariance_measurement_cross[inner][column]; + } + + innovation_covariance[row][column] = matrix_sum; + + if ( row == column ) + { + innovation_covariance[row][column] += + measurement_variance; + } + } + } + +if ( !mekf_matrix_3x3_inverse + ( + innovation_covariance, + inverse_innovation_covariance + ) ) + { + return false; + } + +/* + * Normalized innovation squared: + * + * NIS = transpose(residual) * inverse(S) * residual + * + * Reject a direction measurement that is inconsistent with the estimator's + * predicted uncertainty. This also protects the small-error MEKF linearization + * from very large attitude disagreements. + */ +normalized_innovation_squared = 0.0f; + +for ( row = 0U; row < 3U; row++ ) + { + for ( column = 0U; column < 3U; column++ ) + { + normalized_innovation_squared += + residual[row] * + inverse_innovation_covariance[row][column] * + residual[column]; + } + } + +if ( !isfinite(normalized_innovation_squared) || + normalized_innovation_squared > + filter->config.accelerometer_innovation_gate ) + { + return false; + } + +/* + * Kalman gain: + * + * K = P * transpose(H) * inverse(S) + */ +for ( row = 0U; row < MEKF_ERROR_STATE_DIM; row++ ) + { + for ( column = 0U; column < 3U; column++ ) + { + matrix_sum = 0.0f; + + for ( inner = 0U; inner < 3U; inner++ ) + { + matrix_sum += + covariance_measurement_cross[row][inner] * + inverse_innovation_covariance[inner][column]; + } + + kalman_gain[row][column] = matrix_sum; + } + } + +/* + * Estimate the six-state correction: + * + * delta_x = K * residual + */ +for ( row = 0U; row < MEKF_ERROR_STATE_DIM; row++ ) + { + for ( measurement = 0U; measurement < 3U; measurement++ ) + { + error_state[row] += + kalman_gain[row][measurement] * + residual[measurement]; + } + + if ( !isfinite(error_state[row]) ) + { + return false; + } + } + +/* + * Convert the local attitude-error correction into a quaternion and inject it + * through right multiplication. + */ +correction_magnitude = sqrtf + ( + error_state[MEKF_ATTITUDE_ERROR_X] * + error_state[MEKF_ATTITUDE_ERROR_X] + + error_state[MEKF_ATTITUDE_ERROR_Y] * + error_state[MEKF_ATTITUDE_ERROR_Y] + + error_state[MEKF_ATTITUDE_ERROR_Z] * + error_state[MEKF_ATTITUDE_ERROR_Z] + ); + +if ( !isfinite(correction_magnitude) ) + { + return false; + } + +if ( correction_magnitude <= MEKF_SMALL_ANGLE_RAD ) + { + correction_quaternion.w = 1.0f; + quaternion_vector_scale = 0.5f; + } +else + { + half_correction = 0.5f * correction_magnitude; + + correction_quaternion.w = cosf(half_correction); + + quaternion_vector_scale = + sinf(half_correction) / + correction_magnitude; + } + +correction_quaternion.x = + error_state[MEKF_ATTITUDE_ERROR_X] * + quaternion_vector_scale; + +correction_quaternion.y = + error_state[MEKF_ATTITUDE_ERROR_Y] * + quaternion_vector_scale; + +correction_quaternion.z = + error_state[MEKF_ATTITUDE_ERROR_Z] * + quaternion_vector_scale; + +corrected_attitude = quat_mult + ( + filter->attitude, + correction_quaternion + ); + +corrected_attitude = quat_normalize(corrected_attitude); + +corrected_bias.x = + filter->gyro_bias_rad_s.x + + error_state[MEKF_GYRO_BIAS_ERROR_X]; + +corrected_bias.y = + filter->gyro_bias_rad_s.y + + error_state[MEKF_GYRO_BIAS_ERROR_Y]; + +corrected_bias.z = + filter->gyro_bias_rad_s.z + + error_state[MEKF_GYRO_BIAS_ERROR_Z]; + +if ( !mekf_quat_is_finite(corrected_attitude) || + !mekf_vector_is_finite(corrected_bias) ) + { + return false; + } + +/* + * Use the Joseph covariance update: + * + * A = I - K * H + * + * P_corrected = + * A * P * transpose(A) + + * K * R * transpose(K) + */ +for ( row = 0U; row < MEKF_ERROR_STATE_DIM; row++ ) + { + for ( column = 0U; column < MEKF_ERROR_STATE_DIM; column++ ) + { + if ( row == column ) + { + identity_minus_gain_jacobian[row][column] = 1.0f; + } + + for ( measurement = 0U; measurement < 3U; measurement++ ) + { + identity_minus_gain_jacobian[row][column] -= + kalman_gain[row][measurement] * + measurement_jacobian[measurement][column]; + } + } + } + +for ( row = 0U; row < MEKF_ERROR_STATE_DIM; row++ ) + { + for ( column = 0U; + column < MEKF_ERROR_STATE_DIM; + column++ ) + { + matrix_sum = 0.0f; + + for ( inner = 0U; + inner < MEKF_ERROR_STATE_DIM; + inner++ ) + { + matrix_sum += + identity_minus_gain_jacobian[row][inner] * + filter->covariance[inner][column]; + } + + intermediate_covariance[row][column] = matrix_sum; + } + } + +for ( row = 0U; row < MEKF_ERROR_STATE_DIM; row++ ) + { + for ( column = 0U; + column < MEKF_ERROR_STATE_DIM; + column++ ) + { + matrix_sum = 0.0f; + + for ( inner = 0U; + inner < MEKF_ERROR_STATE_DIM; + inner++ ) + { + matrix_sum += + intermediate_covariance[row][inner] * + identity_minus_gain_jacobian[column][inner]; + } + + joseph_covariance[row][column] = matrix_sum; + + for ( measurement = 0U; measurement < 3U; measurement++ ) + { + joseph_covariance[row][column] += + measurement_variance * + kalman_gain[row][measurement] * + kalman_gain[column][measurement]; + } + } + } + +/* + * Reset the attitude-error state after injecting its correction into the + * nominal quaternion: + * + * G_theta = I - 0.5 * skew(delta_theta) + * + * P_reset = G * P_corrected * transpose(G) + */ +for ( row = 0U; row < MEKF_ERROR_STATE_DIM; row++ ) + { + reset_jacobian[row][row] = 1.0f; + } + +reset_jacobian + [MEKF_ATTITUDE_ERROR_X] + [MEKF_ATTITUDE_ERROR_Y] = + 0.5f * error_state[MEKF_ATTITUDE_ERROR_Z]; + +reset_jacobian + [MEKF_ATTITUDE_ERROR_X] + [MEKF_ATTITUDE_ERROR_Z] = + -0.5f * error_state[MEKF_ATTITUDE_ERROR_Y]; + +reset_jacobian + [MEKF_ATTITUDE_ERROR_Y] + [MEKF_ATTITUDE_ERROR_X] = + -0.5f * error_state[MEKF_ATTITUDE_ERROR_Z]; + +reset_jacobian + [MEKF_ATTITUDE_ERROR_Y] + [MEKF_ATTITUDE_ERROR_Z] = + 0.5f * error_state[MEKF_ATTITUDE_ERROR_X]; + +reset_jacobian + [MEKF_ATTITUDE_ERROR_Z] + [MEKF_ATTITUDE_ERROR_X] = + 0.5f * error_state[MEKF_ATTITUDE_ERROR_Y]; + +reset_jacobian + [MEKF_ATTITUDE_ERROR_Z] + [MEKF_ATTITUDE_ERROR_Y] = + -0.5f * error_state[MEKF_ATTITUDE_ERROR_X]; + +for ( row = 0U; row < MEKF_ERROR_STATE_DIM; row++ ) + { + for ( column = 0U; + column < MEKF_ERROR_STATE_DIM; + column++ ) + { + matrix_sum = 0.0f; + + for ( inner = 0U; + inner < MEKF_ERROR_STATE_DIM; + inner++ ) + { + matrix_sum += + reset_jacobian[row][inner] * + joseph_covariance[inner][column]; + } + + reset_intermediate_covariance[row][column] = + matrix_sum; + } + } + +for ( row = 0U; row < MEKF_ERROR_STATE_DIM; row++ ) + { + for ( column = 0U; + column < MEKF_ERROR_STATE_DIM; + column++ ) + { + matrix_sum = 0.0f; + + for ( inner = 0U; + inner < MEKF_ERROR_STATE_DIM; + inner++ ) + { + matrix_sum += + reset_intermediate_covariance[row][inner] * + reset_jacobian[column][inner]; + } + + corrected_covariance[row][column] = matrix_sum; + } + } + +/* + * Remove small floating-point asymmetry and verify the complete result before + * committing any part of the correction. + */ +for ( row = 0U; row < MEKF_ERROR_STATE_DIM; row++ ) + { + for ( column = row + 1U; + column < MEKF_ERROR_STATE_DIM; + column++ ) + { + symmetric_value = + 0.5f * + ( + corrected_covariance[row][column] + + corrected_covariance[column][row] + ); + + corrected_covariance[row][column] = symmetric_value; + corrected_covariance[column][row] = symmetric_value; + } + } + +if ( !mekf_covariance_is_finite(corrected_covariance) ) + { + return false; + } + +filter->attitude = corrected_attitude; +filter->gyro_bias_rad_s = corrected_bias; + +for ( row = 0U; row < MEKF_ERROR_STATE_DIM; row++ ) + { + for ( column = 0U; + column < MEKF_ERROR_STATE_DIM; + column++ ) + { + filter->covariance[row][column] = + corrected_covariance[row][column]; + } + } + +return true; + +} /* mekf_update_accelerometer */ + /******************************************************************************* * END OF FILE ******************************************************************************/ \ No newline at end of file diff --git a/mekf/mekf.h b/mekf/mekf.h index 9421cd0..3eda58f 100644 --- a/mekf/mekf.h +++ b/mekf/mekf.h @@ -68,7 +68,7 @@ typedef enum _MEKF_ERROR_STATE_INDEX } MEKF_ERROR_STATE_INDEX; /** - * @brief Initial uncertainty and gyro prediction configuration. + * @brief MEKF initialization, prediction, and measurement-update configuration. */ typedef struct _MEKF_CONFIG { @@ -121,6 +121,46 @@ typedef struct _MEKF_CONFIG */ float gyro_bias_random_walk_rad_s2_sqrt_hz; + /** + * One-sigma uncertainty of each component of the normalized accelerometer + * direction measurement. + * + * The accelerometer update compares normalized measured and predicted gravity + * directions, so this value is dimensionless. A larger value makes the filter + * trust accelerometer direction less strongly. + */ + float accelerometer_direction_std; + + /** + * Expected local gravitational acceleration magnitude in meters per second + * squared. + * + * This value is used to determine whether the measured acceleration magnitude + * is sufficiently close to gravity for attitude correction. + */ + float gravity_magnitude_m_s2; + + /** + * Maximum permitted absolute difference between measured acceleration + * magnitude and gravity magnitude, in meters per second squared. + * + * Measurements outside this range are rejected because vehicle acceleration, + * vibration, or free fall makes the accelerometer unreliable as a gravity + * reference. + */ + float accelerometer_magnitude_tolerance_m_s2; + + /** + * Maximum permitted normalized innovation squared for an accelerometer + * measurement. + * + * This rejects gravity-direction residuals that are inconsistent with the + * predicted covariance and configured accelerometer uncertainty. A value of + * approximately 11.345 corresponds to a 99 percent chi-square threshold for + * a three-component residual. + */ + float accelerometer_innovation_gate; + /** * Maximum valid gyro prediction timestep in seconds. * This represents the largest allowable time interval for one prediction. @@ -225,6 +265,30 @@ bool mekf_predict float delta_time_s ); +/** + * @brief Corrects attitude and gyro bias using a body-frame accelerometer + * measurement. + * + * The acceleration measurement is normalized and compared with the predicted + * body-frame gravity direction. Correction is applied only when its magnitude + * is sufficiently close to the configured gravity magnitude. + * + * Accelerometer correction constrains tilt relative to gravity but cannot + * independently observe rotation about the gravity vector. + * + * @param filter Initialized filter instance. + * @param acceleration_body_m_s2 Body-frame accelerometer measurement in meters + * per second squared. + * + * @return true when the measurement is accepted and the update succeeds; + * otherwise false. + */ +bool mekf_update_accelerometer + ( + MEKF_FILTER *filter, + VECTOR_3F acceleration_body_m_s2 + ); + #ifdef __cplusplus } #endif diff --git a/test/mekf/test_mekf.c b/test/mekf/test_mekf.c index 4a691f5..cc0659d 100644 --- a/test/mekf/test_mekf.c +++ b/test/mekf/test_mekf.c @@ -49,6 +49,10 @@ MEKF_CONFIG config = }, .gyro_noise_density_rad_s_sqrt_hz = 0.005f, .gyro_bias_random_walk_rad_s2_sqrt_hz = 0.0001f, + .accelerometer_direction_std = 0.05f, + .gravity_magnitude_m_s2 = 9.80665f, + .accelerometer_magnitude_tolerance_m_s2 = 1.50f, + .accelerometer_innovation_gate = 11.344867f, .maximum_delta_time_s = 0.10f }; @@ -331,7 +335,7 @@ for ( row = 0U; row < MEKF_ERROR_STATE_DIM; row++ ) } /* test_mekf_init_sets_diagonal_covariance */ /** - * @brief Verifies that prediction configuration is copied into the filter. + * @brief Verifies that MEKF configuration is copied into the filter. */ void test_mekf_init_copies_config ( @@ -370,6 +374,34 @@ TEST_ASSERT_EQ_FLOAT config.gyro_bias_random_walk_rad_s2_sqrt_hz ); +TEST_ASSERT_EQ_FLOAT + ( + "Accelerometer direction uncertainty is copied", + filter.config.accelerometer_direction_std, + config.accelerometer_direction_std + ); + +TEST_ASSERT_EQ_FLOAT + ( + "Gravity magnitude is copied", + filter.config.gravity_magnitude_m_s2, + config.gravity_magnitude_m_s2 + ); + +TEST_ASSERT_EQ_FLOAT + ( + "Accelerometer magnitude tolerance is copied", + filter.config.accelerometer_magnitude_tolerance_m_s2, + config.accelerometer_magnitude_tolerance_m_s2 + ); + +TEST_ASSERT_EQ_FLOAT + ( + "Accelerometer innovation gate is copied", + filter.config.accelerometer_innovation_gate, + config.accelerometer_innovation_gate + ); + TEST_ASSERT_EQ_FLOAT ( "Maximum timestep is copied", @@ -584,6 +616,143 @@ TEST_ASSERT_TRUE } /* test_mekf_init_rejects_invalid_process_noise */ +/** + * @brief Verifies that invalid accelerometer-update configuration values are + * rejected. + */ +void test_mekf_init_rejects_invalid_accelerometer_config + ( + void + ) +{ +MEKF_FILTER filter; +MEKF_CONFIG config; + +QUAT identity = { 1.0f, 0.0f, 0.0f, 0.0f }; +VECTOR_3F zero_bias = { 0.0f, 0.0f, 0.0f }; + +/* + * Accelerometer direction uncertainty must be finite and greater than zero. + */ +config = make_valid_config(); +config.accelerometer_direction_std = 0.0f; + +TEST_ASSERT_FALSE + ( + "Zero accelerometer direction uncertainty is rejected", + mekf_init(&filter, identity, zero_bias, &config) + ); + +config = make_valid_config(); +config.accelerometer_direction_std = -0.01f; + +TEST_ASSERT_FALSE + ( + "Negative accelerometer direction uncertainty is rejected", + mekf_init(&filter, identity, zero_bias, &config) + ); + +config = make_valid_config(); +config.accelerometer_direction_std = NAN; + +TEST_ASSERT_FALSE + ( + "Nonfinite accelerometer direction uncertainty is rejected", + mekf_init(&filter, identity, zero_bias, &config) + ); + +/* + * Expected gravity magnitude must be finite and greater than zero. + */ +config = make_valid_config(); +config.gravity_magnitude_m_s2 = 0.0f; + +TEST_ASSERT_FALSE + ( + "Zero gravity magnitude is rejected", + mekf_init(&filter, identity, zero_bias, &config) + ); + +config = make_valid_config(); +config.gravity_magnitude_m_s2 = -9.80665f; + +TEST_ASSERT_FALSE + ( + "Negative gravity magnitude is rejected", + mekf_init(&filter, identity, zero_bias, &config) + ); + +config = make_valid_config(); +config.gravity_magnitude_m_s2 = NAN; + +TEST_ASSERT_FALSE + ( + "Nonfinite gravity magnitude is rejected", + mekf_init(&filter, identity, zero_bias, &config) + ); + +/* + * The accelerometer magnitude tolerance must also be finite and positive. + */ +config = make_valid_config(); +config.accelerometer_magnitude_tolerance_m_s2 = 0.0f; + +TEST_ASSERT_FALSE + ( + "Zero accelerometer magnitude tolerance is rejected", + mekf_init(&filter, identity, zero_bias, &config) + ); + +config = make_valid_config(); +config.accelerometer_magnitude_tolerance_m_s2 = -1.0f; + +TEST_ASSERT_FALSE + ( + "Negative accelerometer magnitude tolerance is rejected", + mekf_init(&filter, identity, zero_bias, &config) + ); + +config = make_valid_config(); +config.accelerometer_magnitude_tolerance_m_s2 = NAN; + +TEST_ASSERT_FALSE + ( + "Nonfinite accelerometer magnitude tolerance is rejected", + mekf_init(&filter, identity, zero_bias, &config) + ); + +/* + * The accelerometer innovation gate must be finite and greater than zero. + */ +config = make_valid_config(); +config.accelerometer_innovation_gate = 0.0f; + +TEST_ASSERT_FALSE + ( + "Zero accelerometer innovation gate is rejected", + mekf_init(&filter, identity, zero_bias, &config) + ); + +config = make_valid_config(); +config.accelerometer_innovation_gate = -1.0f; + +TEST_ASSERT_FALSE + ( + "Negative accelerometer innovation gate is rejected", + mekf_init(&filter, identity, zero_bias, &config) + ); + +config = make_valid_config(); +config.accelerometer_innovation_gate = NAN; + +TEST_ASSERT_FALSE + ( + "Nonfinite accelerometer innovation gate is rejected", + mekf_init(&filter, identity, zero_bias, &config) + ); + +} /* test_mekf_init_rejects_invalid_accelerometer_config */ + /** * @brief Verifies rejection of invalid maximum timestep values. */ @@ -1420,28 +1589,686 @@ for ( row = 0U; row < MEKF_ERROR_STATE_DIM; row++ ) } /* test_mekf_predict_rotates_attitude_covariance */ /*------------------------------------------------------------------------------ - Main + Accelerometer Update Tests ------------------------------------------------------------------------------*/ -int main +/** + * @brief Verifies that a gravity measurement aligned with the predicted + * direction does not change the nominal attitude or gyro-bias estimate. + */ +void test_mekf_update_accelerometer_aligned_measurement ( void ) { -unit_test tests[] = - { - { - "mekf_init_identity_and_bias", - test_mekf_init_identity_and_bias - }, - { - "mekf_init_normalizes_attitude", - test_mekf_init_normalizes_attitude - }, +MEKF_FILTER filter; +MEKF_CONFIG config = make_valid_config(); + +QUAT identity = { 1.0f, 0.0f, 0.0f, 0.0f }; +VECTOR_3F zero_bias = { 0.0f, 0.0f, 0.0f }; + +VECTOR_3F aligned_gravity = { - "mekf_init_zero_quaternion_uses_identity", - test_mekf_init_zero_quaternion_uses_identity - }, + .x = 0.0f, + .y = 0.0f, + .z = 9.80665f + }; + +TEST_ASSERT_TRUE + ( + "MEKF initialization succeeds", + mekf_init + ( + &filter, + identity, + zero_bias, + &config + ) + ); + +TEST_ASSERT_TRUE + ( + "Aligned accelerometer measurement is accepted", + mekf_update_accelerometer + ( + &filter, + aligned_gravity + ) + ); + +assert_quat_components + ( + "Aligned measurement leaves attitude unchanged", + filter.attitude, + identity + ); + +assert_vector_components + ( + "Aligned measurement leaves gyro bias unchanged", + filter.gyro_bias_rad_s, + zero_bias + ); + +} /* test_mekf_update_accelerometer_aligned_measurement */ + +/** + * @brief Verifies that a tilted gravity measurement moves the attitude estimate + * toward the measured gravity direction without introducing yaw correction. + */ +void test_mekf_update_accelerometer_corrects_tilt + ( + void + ) +{ +MEKF_FILTER filter; +MEKF_CONFIG config = make_valid_config(); + +QUAT identity = { 1.0f, 0.0f, 0.0f, 0.0f }; +QUAT gravity_world = { 0.0f, 0.0f, 0.0f, 1.0f }; +QUAT predicted_gravity_after; + +VECTOR_3F zero_bias = { 0.0f, 0.0f, 0.0f }; + +VECTOR_3F tilted_gravity; + +float tilt_angle_rad = 0.174532925f; +float direction_error_before; +float direction_error_after; +float quaternion_norm_squared; + +/* + * Give the filter meaningful tilt uncertainty so the accelerometer measurement + * produces a visible correction. + */ +config.initial_attitude_std_rad.x = 0.20f; +config.initial_attitude_std_rad.y = 0.20f; +config.initial_attitude_std_rad.z = 0.20f; + +/* + * Construct a gravity measurement tilted ten degrees toward positive body Y. + * Its magnitude remains exactly equal to the configured gravity magnitude. + */ +tilted_gravity.x = 0.0f; + +tilted_gravity.y = + config.gravity_magnitude_m_s2 * + sinf(tilt_angle_rad); + +tilted_gravity.z = + config.gravity_magnitude_m_s2 * + cosf(tilt_angle_rad); + +direction_error_before = sinf(tilt_angle_rad); + +TEST_ASSERT_TRUE + ( + "MEKF initialization succeeds", + mekf_init + ( + &filter, + identity, + zero_bias, + &config + ) + ); + +TEST_ASSERT_TRUE + ( + "Tilted accelerometer measurement is accepted", + mekf_update_accelerometer + ( + &filter, + tilted_gravity + ) + ); + +predicted_gravity_after = quat_rotate_world_to_body + ( + filter.attitude, + gravity_world + ); + +direction_error_after = fabsf + ( + sinf(tilt_angle_rad) - + predicted_gravity_after.y + ); + +quaternion_norm_squared = + 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 + ( + "Tilt correction rotates about positive body X", + filter.attitude.x > 0.0f + ); + +TEST_ASSERT_TRUE + ( + "Tilt correction reduces gravity-direction error", + direction_error_after < direction_error_before + ); + +TEST_ASSERT_TRUE + ( + "Accelerometer does not introduce yaw correction", + fabsf(filter.attitude.z) < 1.0e-6f + ); + +TEST_ASSERT_TRUE + ( + "Corrected quaternion remains normalized", + fabsf(quaternion_norm_squared - 1.0f) < 1.0e-5f + ); + +assert_vector_components + ( + "Tilt correction leaves uncoupled gyro bias unchanged", + filter.gyro_bias_rad_s, + zero_bias + ); + +} /* test_mekf_update_accelerometer_corrects_tilt */ + +/** + * @brief Verifies that acceleration outside the configured gravity-magnitude + * gate is rejected without modifying the filter. + */ +void test_mekf_update_accelerometer_rejects_dynamic_acceleration + ( + void + ) +{ +unsigned int row; +unsigned int column; + +MEKF_FILTER filter; +MEKF_FILTER original_filter; +MEKF_CONFIG config = make_valid_config(); + +QUAT identity = { 1.0f, 0.0f, 0.0f, 0.0f }; +VECTOR_3F zero_bias = { 0.0f, 0.0f, 0.0f }; + +VECTOR_3F dynamic_acceleration = + { + 0.0f, + 0.0f, + 0.0f + }; + +dynamic_acceleration.z = + config.gravity_magnitude_m_s2 + + config.accelerometer_magnitude_tolerance_m_s2 + + 0.10f; + +TEST_ASSERT_TRUE + ( + "MEKF initialization succeeds", + mekf_init + ( + &filter, + identity, + zero_bias, + &config + ) + ); + +original_filter = filter; + +TEST_ASSERT_FALSE + ( + "Dynamic acceleration is rejected", + mekf_update_accelerometer + ( + &filter, + dynamic_acceleration + ) + ); + +assert_quat_components + ( + "Rejected measurement leaves attitude unchanged", + filter.attitude, + original_filter.attitude + ); + +assert_vector_components + ( + "Rejected measurement leaves gyro bias unchanged", + filter.gyro_bias_rad_s, + original_filter.gyro_bias_rad_s + ); + +for ( row = 0U; row < MEKF_ERROR_STATE_DIM; row++ ) + { + for ( column = 0U; + column < MEKF_ERROR_STATE_DIM; + column++ ) + { + TEST_ASSERT_EQ_FLOAT + ( + "Rejected measurement leaves covariance unchanged", + filter.covariance[row][column], + original_filter.covariance[row][column] + ); + } + } + +} /* test_mekf_update_accelerometer_rejects_dynamic_acceleration */ + +/** + * @brief Verifies that an aligned accelerometer update reduces observable tilt + * uncertainty without reducing unobservable yaw uncertainty. + */ +void test_mekf_update_accelerometer_updates_covariance + ( + void + ) +{ +unsigned int row; +unsigned int column; + +MEKF_FILTER filter; +MEKF_CONFIG config = make_valid_config(); + +float expected[MEKF_ERROR_STATE_DIM][MEKF_ERROR_STATE_DIM] = + { + { 0.0f } + }; + +QUAT identity = { 1.0f, 0.0f, 0.0f, 0.0f }; +VECTOR_3F zero_vector = { 0.0f, 0.0f, 0.0f }; + +VECTOR_3F aligned_gravity = + { + 0.0f, + 0.0f, + 9.80665f + }; + +/* + * Initial attitude variance: + * + * p = 0.2^2 = 0.04 + * + * Direction-measurement variance: + * + * r = 0.1^2 = 0.01 + * + * For each observable tilt axis: + * + * p_new = p * r / (p + r) = 0.008 + */ +config.initial_attitude_std_rad.x = 0.20f; +config.initial_attitude_std_rad.y = 0.20f; +config.initial_attitude_std_rad.z = 0.20f; + +config.initial_gyro_bias_std_rad_s = zero_vector; +config.accelerometer_direction_std = 0.10f; + +TEST_ASSERT_TRUE + ( + "MEKF initialization succeeds", + mekf_init + ( + &filter, + identity, + zero_vector, + &config + ) + ); + +TEST_ASSERT_TRUE + ( + "Aligned covariance update succeeds", + mekf_update_accelerometer + ( + &filter, + aligned_gravity + ) + ); + +expected[MEKF_ATTITUDE_ERROR_X][MEKF_ATTITUDE_ERROR_X] = 0.008f; +expected[MEKF_ATTITUDE_ERROR_Y][MEKF_ATTITUDE_ERROR_Y] = 0.008f; + +/* + * Gravity cannot observe rotation about the gravity vector, so the Z-axis + * attitude variance remains at its initial value. + */ +expected[MEKF_ATTITUDE_ERROR_Z][MEKF_ATTITUDE_ERROR_Z] = 0.040f; + +for ( row = 0U; row < MEKF_ERROR_STATE_DIM; row++ ) + { + for ( column = 0U; + column < MEKF_ERROR_STATE_DIM; + column++ ) + { + TEST_ASSERT_EQ_FLOAT + ( + "Accelerometer-updated covariance entry", + filter.covariance[row][column], + expected[row][column] + ); + } + } + +} /* test_mekf_update_accelerometer_updates_covariance */ + +/** + * @brief Verifies that accelerometer correction can update gyro bias through + * attitude-to-bias cross-covariance created during prediction. + */ +void test_mekf_update_accelerometer_corrects_coupled_gyro_bias + ( + void + ) +{ +MEKF_FILTER filter; +MEKF_CONFIG config = make_valid_config(); + +QUAT identity = { 1.0f, 0.0f, 0.0f, 0.0f }; + +VECTOR_3F zero_vector = { 0.0f, 0.0f, 0.0f }; +VECTOR_3F tilted_gravity; + +float tilt_angle_rad = 0.087266463f; + +/* + * Begin with both attitude and gyro-bias uncertainty. Prediction will create + * negative attitude-to-bias cross-covariance. + */ +config.initial_attitude_std_rad.x = 0.20f; +config.initial_attitude_std_rad.y = 0.20f; +config.initial_attitude_std_rad.z = 0.20f; + +config.initial_gyro_bias_std_rad_s.x = 0.10f; +config.initial_gyro_bias_std_rad_s.y = 0.10f; +config.initial_gyro_bias_std_rad_s.z = 0.10f; + +config.gyro_noise_density_rad_s_sqrt_hz = 0.0f; +config.gyro_bias_random_walk_rad_s2_sqrt_hz = 0.0f; +config.accelerometer_direction_std = 0.10f; +config.maximum_delta_time_s = 0.10f; + +tilted_gravity.x = 0.0f; + +tilted_gravity.y = + config.gravity_magnitude_m_s2 * + sinf(tilt_angle_rad); + +tilted_gravity.z = + config.gravity_magnitude_m_s2 * + cosf(tilt_angle_rad); + +TEST_ASSERT_TRUE + ( + "MEKF initialization succeeds", + mekf_init + ( + &filter, + identity, + zero_vector, + &config + ) + ); + +TEST_ASSERT_TRUE + ( + "Zero-rate prediction creates bias coupling", + mekf_predict + ( + &filter, + zero_vector, + 0.10f + ) + ); + +TEST_ASSERT_TRUE + ( + "Coupled accelerometer update succeeds", + mekf_update_accelerometer + ( + &filter, + tilted_gravity + ) + ); + +TEST_ASSERT_TRUE + ( + "Accelerometer applies positive X tilt correction", + filter.attitude.x > 0.0f + ); + +TEST_ASSERT_TRUE + ( + "Coupled X gyro-bias estimate is corrected", + filter.gyro_bias_rad_s.x < 0.0f + ); + +TEST_ASSERT_TRUE + ( + "Uncoupled Y gyro-bias remains zero", + fabsf(filter.gyro_bias_rad_s.y) < 1.0e-6f + ); + +TEST_ASSERT_TRUE + ( + "Unobservable Z gyro-bias remains zero", + fabsf(filter.gyro_bias_rad_s.z) < 1.0e-6f + ); + +} /* test_mekf_update_accelerometer_corrects_coupled_gyro_bias */ + +/** + * @brief Verifies that null, zero-magnitude, and nonfinite accelerometer inputs + * are rejected without modifying the filter. + */ +void test_mekf_update_accelerometer_rejects_invalid_input + ( + void + ) +{ +unsigned int row; +unsigned int column; + +MEKF_FILTER filter; +MEKF_FILTER original_filter; +MEKF_CONFIG config = make_valid_config(); + +QUAT identity = { 1.0f, 0.0f, 0.0f, 0.0f }; + +VECTOR_3F zero_vector = { 0.0f, 0.0f, 0.0f }; + +VECTOR_3F nonfinite_acceleration = + { + NAN, + 0.0f, + 9.80665f + }; + +TEST_ASSERT_TRUE + ( + "MEKF initialization succeeds", + mekf_init + ( + &filter, + identity, + zero_vector, + &config + ) + ); + +original_filter = filter; + +TEST_ASSERT_FALSE + ( + "Null filter is rejected", + mekf_update_accelerometer + ( + NULL, + zero_vector + ) + ); + +TEST_ASSERT_FALSE + ( + "Zero-magnitude acceleration is rejected", + mekf_update_accelerometer + ( + &filter, + zero_vector + ) + ); + +TEST_ASSERT_FALSE + ( + "Nonfinite acceleration is rejected", + mekf_update_accelerometer + ( + &filter, + nonfinite_acceleration + ) + ); + +assert_quat_components + ( + "Invalid input leaves attitude unchanged", + filter.attitude, + original_filter.attitude + ); + +assert_vector_components + ( + "Invalid input leaves gyro bias unchanged", + filter.gyro_bias_rad_s, + original_filter.gyro_bias_rad_s + ); + +for ( row = 0U; row < MEKF_ERROR_STATE_DIM; row++ ) + { + for ( column = 0U; + column < MEKF_ERROR_STATE_DIM; + column++ ) + { + TEST_ASSERT_EQ_FLOAT + ( + "Invalid input leaves covariance unchanged", + filter.covariance[row][column], + original_filter.covariance[row][column] + ); + } + } + +} /* test_mekf_update_accelerometer_rejects_invalid_input */ + +/** + * @brief Verifies that a gravity measurement pointing opposite the predicted + * direction is rejected without modifying the filter. + */ +void test_mekf_update_accelerometer_rejects_large_direction_error + ( + void + ) +{ +unsigned int row; +unsigned int column; + +MEKF_FILTER filter; +MEKF_FILTER original_filter; +MEKF_CONFIG config = make_valid_config(); + +QUAT identity = { 1.0f, 0.0f, 0.0f, 0.0f }; +VECTOR_3F zero_vector = { 0.0f, 0.0f, 0.0f }; + +VECTOR_3F opposite_gravity = + { + 0.0f, + 0.0f, + -9.80665f + }; + +TEST_ASSERT_TRUE + ( + "MEKF initialization succeeds", + mekf_init + ( + &filter, + identity, + zero_vector, + &config + ) + ); + +original_filter = filter; + +TEST_ASSERT_FALSE + ( + "Opposite gravity direction is rejected", + mekf_update_accelerometer + ( + &filter, + opposite_gravity + ) + ); + +assert_quat_components + ( + "Rejected direction leaves attitude unchanged", + filter.attitude, + original_filter.attitude + ); + +assert_vector_components + ( + "Rejected direction leaves gyro bias unchanged", + filter.gyro_bias_rad_s, + original_filter.gyro_bias_rad_s + ); + +for ( row = 0U; row < MEKF_ERROR_STATE_DIM; row++ ) + { + for ( column = 0U; + column < MEKF_ERROR_STATE_DIM; + column++ ) + { + TEST_ASSERT_EQ_FLOAT + ( + "Rejected direction leaves covariance unchanged", + filter.covariance[row][column], + original_filter.covariance[row][column] + ); + } + } + +} /* test_mekf_update_accelerometer_rejects_large_direction_error */ + +/*------------------------------------------------------------------------------ + Main + ------------------------------------------------------------------------------*/ + +int main + ( + void + ) +{ +unit_test tests[] = + { + { + "mekf_init_identity_and_bias", + test_mekf_init_identity_and_bias + }, + { + "mekf_init_normalizes_attitude", + test_mekf_init_normalizes_attitude + }, + { + "mekf_init_zero_quaternion_uses_identity", + test_mekf_init_zero_quaternion_uses_identity + }, { "mekf_init_sets_diagonal_covariance", test_mekf_init_sets_diagonal_covariance @@ -1467,6 +2294,10 @@ unit_test tests[] = test_mekf_init_rejects_invalid_process_noise }, { + "mekf_init_rejects_invalid_accelerometer_config", + test_mekf_init_rejects_invalid_accelerometer_config + }, + { "mekf_init_rejects_invalid_maximum_timestep", test_mekf_init_rejects_invalid_maximum_timestep }, @@ -1505,7 +2336,35 @@ unit_test tests[] = { "mekf_predict_rotates_attitude_covariance", test_mekf_predict_rotates_attitude_covariance - } + }, + { + "mekf_update_accelerometer_corrects_tilt", + test_mekf_update_accelerometer_corrects_tilt + }, + { + "mekf_update_accelerometer_aligned_measurement", + test_mekf_update_accelerometer_aligned_measurement + }, + { + "mekf_update_accelerometer_rejects_dynamic_acceleration", + test_mekf_update_accelerometer_rejects_dynamic_acceleration + }, + { + "mekf_update_accelerometer_updates_covariance", + test_mekf_update_accelerometer_updates_covariance + }, + { + "mekf_update_accelerometer_corrects_coupled_gyro_bias", + test_mekf_update_accelerometer_corrects_coupled_gyro_bias + }, + { + "mekf_update_accelerometer_rejects_invalid_input", + test_mekf_update_accelerometer_rejects_invalid_input + }, + { + "mekf_update_accelerometer_rejects_large_direction_error", + test_mekf_update_accelerometer_rejects_large_direction_error + }, }; TEST_INITIALIZE_TEST("mekf.c", tests); From c327590831393d69c35fa4873eebdedd15bc3f4f Mon Sep 17 00:00:00 2001 From: Bjorn Bengtsson Date: Thu, 13 Aug 2026 20:51:37 -0700 Subject: [PATCH 6/6] Fix MEKF transition matrix comment --- mekf/mekf.c | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/mekf/mekf.c b/mekf/mekf.c index 7422552..9f13a60 100644 --- a/mekf/mekf.c +++ b/mekf/mekf.c @@ -675,9 +675,9 @@ predicted_attitude = quat_normalize(predicted_attitude); * partly along another axis after rotation. * * Constructs: - * -[ω]× * Δt = [ 0 -ω_z*Δt ω_y*Δt ] - * [ ω_z*Δt 0 -ω_x*Δt ] - * [ -ω_y*Δt ω_x*Δt 0 ] + * -[ω]× * Δt = [ 0 ω_z*Δt -ω_y*Δt ] + * [ -ω_z*Δt 0 ω_x*Δt ] + * [ ω_y*Δt -ω_x*Δt 0 ] */ for ( row = 0U; row < MEKF_ERROR_STATE_DIM; row++ ) {