fix(filter): Add PT1 smoothing to Kalman gain

src/filter/filter.c

@@ -25,10 +25,6 @@ volatile filter_config_t filterConfig =
 
 // PT1 Low Pass filter
 bool acc_filter_initialized = false;
-typedef struct pt1Filter_s {
-    float state;
-    float k;
-} pt1Filter_t;
 
 pt1Filter_t ax_filter;
 pt1Filter_t ay_filter;
@@ -98,7 +94,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)

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);

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;

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

src/version.h

@@ -3,4 +3,4 @@
 #define HARDWARE_VERSION 101
 #define BOOTLOADER_VERSION 101
 
-#define FIRMWARE_VERSION 256
+#define FIRMWARE_VERSION 258