From e2eeeff7bacb0dc28cd15020bdc30d3800a40daa Mon Sep 17 00:00:00 2001 From: nerdCopter <56646290+nerdCopter@users.noreply.github.com> Date: Sat, 25 Oct 2025 13:24:38 -0500 Subject: [PATCH 1/3] fix(filter): Correct Kalman covariance update signal Description: The filtered gyro output from the IMU was observed to be significantly noisier than the output from a standard EmuFlight FC, even with comparable tunes. This resulted in a less effective overall filtering pipeline for HELIOSPRING boards. The root cause was traced to the signal used to adapt the Kalman filter's measurement noise covariance (r). The update_kalman_covariance() function was being fed the raw, unfiltered gyro data. This caused the calculated noise variance to remain high, which in turn kept the Kalman gain high, allowing more noise to pass through the filter. This change corrects the signal flow by moving the update_kalman_covariance() call to after the PTN filter has run. The function is now fed the cleaner, filtered gyro data. This mirrors the more effective implementation in the main EmuFlight firmware, creating a self-reinforcing loop where a cleaner signal leads to a lower Kalman gain and more aggressive noise rejection. This should align the IMU's filtering performance with the expected "floored" noise spectrum seen on other flight controllers. bump version 257 --- src/filter/filter.c | 2 +- src/version.h | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/filter/filter.c b/src/filter/filter.c index 81b5c9c..6f51789 100644 --- a/src/filter/filter.c +++ b/src/filter/filter.c @@ -98,7 +98,7 @@ void filter_data(volatile axisData_t *gyroRateData, volatile axisData_t *gyroAcc filteredData->rateData.y = ptnFilterApply(filteredData->rateData.y, &(lpfFilterStateRate.y)); filteredData->rateData.z = ptnFilterApply(filteredData->rateData.z, &(lpfFilterStateRate.z)); - update_kalman_covariance(gyroRateData); + update_kalman_covariance(&(filteredData->rateData)); if (setPointNew) diff --git a/src/version.h b/src/version.h index ac18910..a5c0891 100644 --- a/src/version.h +++ b/src/version.h @@ -3,4 +3,4 @@ #define HARDWARE_VERSION 101 #define BOOTLOADER_VERSION 101 -#define FIRMWARE_VERSION 256 +#define FIRMWARE_VERSION 257 From afea17f7aed646134f823965d9f34ba7499e526e Mon Sep 17 00:00:00 2001 From: nerdCopter <56646290+nerdCopter@users.noreply.github.com> Date: Sat, 25 Oct 2025 13:58:35 -0500 Subject: [PATCH 2/3] feat(filter): Add PT1 smoothing to Kalman gain Add temporal smoothing to Kalman filter gain using PT1 low-pass filter. This aligns IMU-F implementation with EmuFlight's proven Kalman design. Changes: - Export pt1Filter_t type and functions from filter.h - Add pt1Filter_t kFilter to kalman_t structure - Initialize PT1 filter with 50 Hz cutoff in init_kalman() - Apply PT1 filter to calculated gain in kalman_process() Benefits: - Smoother filter transitions - Reduced oscillation risk - More predictable filter behavior - Complete alignment with EmuFlight implementation This enhancement builds upon the covariance signal fix (v257) and provides additional stability through gain smoothing. Bump version to 258 --- src/filter/filter.c | 6 +----- src/filter/filter.h | 11 +++++++++++ src/filter/kalman.c | 5 +++++ src/filter/kalman.h | 1 + src/version.h | 2 +- 5 files changed, 19 insertions(+), 6 deletions(-) diff --git a/src/filter/filter.c b/src/filter/filter.c index 6f51789..98ff61f 100644 --- a/src/filter/filter.c +++ b/src/filter/filter.c @@ -23,12 +23,8 @@ volatile filter_config_t filterConfig = 3, //init defaults for: uint16_t ptnFilterType; }; -// PT1 Low Pass filter +// PT1 Low Pass filter (type now in filter.h) bool acc_filter_initialized = false; -typedef struct pt1Filter_s { - float state; - float k; -} pt1Filter_t; pt1Filter_t ax_filter; pt1Filter_t ay_filter; diff --git a/src/filter/filter.h b/src/filter/filter.h index b613464..50c3e2d 100644 --- a/src/filter/filter.h +++ b/src/filter/filter.h @@ -8,6 +8,12 @@ #define DEFAULT_YAW_Q 3000 #define BASE_LPF_HZ 70.0f +// PT1 Low Pass filter +typedef struct pt1Filter_s { + float state; + float k; +} pt1Filter_t; + typedef enum filterAxisTypedef { ROLL = 0, PITCH = 1, @@ -41,4 +47,9 @@ extern void allow_filter_init(void); extern void filter_init(void); extern void filter_data(volatile axisData_t* gyroRateData, volatile axisData_t* gyroAccData, float gyroTempData, filteredData_t* filteredData); +// PT1 filter functions +extern float pt1FilterGain(uint16_t f_cut, float dT); +extern void pt1FilterInit(pt1Filter_t *filter, float k, float val); +extern float pt1FilterApply(pt1Filter_t *filter, float input); + void filter_acc(volatile axisData_t *gyroAccData); diff --git a/src/filter/kalman.c b/src/filter/kalman.c index 59d6dad..9e0cf02 100644 --- a/src/filter/kalman.c +++ b/src/filter/kalman.c @@ -14,6 +14,10 @@ void init_kalman(kalman_t *filter, float q) filter->r = 88.0f; //seeding R at 88.0f filter->p = 30.0f; //seeding P at 30.0f filter->e = 1.0f; + + // Initialize PT1 filter for Kalman gain smoothing + // 50 Hz cutoff with 32kHz sample rate (REFRESH_RATE = 1/32000 = 0.00003125s) + pt1FilterInit(&filter->kFilter, pt1FilterGain(50, REFRESH_RATE), 0.0f); } void kalman_init(void) @@ -104,6 +108,7 @@ inline float kalman_process(kalman_t* kalmanState, volatile float input) { //measurement update kalmanState->k = kalmanState->p / (kalmanState->p + 10.0f); + kalmanState->k = pt1FilterApply(&kalmanState->kFilter, kalmanState->k); kalmanState->x += kalmanState->k * (input - kalmanState->x); kalmanState->p = (1.0f - kalmanState->k) * kalmanState->p; return kalmanState->x; diff --git a/src/filter/kalman.h b/src/filter/kalman.h index f90859b..a3d787d 100644 --- a/src/filter/kalman.h +++ b/src/filter/kalman.h @@ -20,6 +20,7 @@ typedef struct kalman float lastX; //previous state float e; //multiplier or adder to q float s; //changes how dynamic e is + pt1Filter_t kFilter; //PT1 filter for gain smoothing } kalman_t; typedef struct variance diff --git a/src/version.h b/src/version.h index a5c0891..dda2d57 100644 --- a/src/version.h +++ b/src/version.h @@ -3,4 +3,4 @@ #define HARDWARE_VERSION 101 #define BOOTLOADER_VERSION 101 -#define FIRMWARE_VERSION 257 +#define FIRMWARE_VERSION 258 From 2ebc06a38fc8e1dd83aed121eb8cfc4c1b993e99 Mon Sep 17 00:00:00 2001 From: nerdCopter <56646290+nerdCopter@users.noreply.github.com> Date: Wed, 5 Nov 2025 15:02:30 -0600 Subject: [PATCH 3/3] remove AI comment --- src/filter/filter.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/filter/filter.c b/src/filter/filter.c index 98ff61f..a24e74c 100644 --- a/src/filter/filter.c +++ b/src/filter/filter.c @@ -23,7 +23,7 @@ volatile filter_config_t filterConfig = 3, //init defaults for: uint16_t ptnFilterType; }; -// PT1 Low Pass filter (type now in filter.h) +// PT1 Low Pass filter bool acc_filter_initialized = false; pt1Filter_t ax_filter;