Update bbn_wave_freq_m5atomS3.ino

bareboat-necessities · Aug 28, 2024 · b1f00a2 · b1f00a2
1 parent e317ffc
commit b1f00a2
Showing 1 changed file with 13 additions and 0 deletions.
diff --git a/bbn_wave_freq_m5atomS3/bbn_wave_freq_m5atomS3.ino b/bbn_wave_freq_m5atomS3/bbn_wave_freq_m5atomS3.ino
@@ -18,6 +18,7 @@
 
 #include <M5Unified.h>
 #include "AranovskiyFilter.h"
+#include "KalmanSmoother.h"
 
 unsigned long now = 0UL, last_refresh = 0UL, last_update = 0UL;
 int got_samples = 0;
@@ -35,6 +36,10 @@ double sigma_0 = theta_0;
 
 double delta_t;  // time step sec
 
+KalmanSmootherVars kalman;
+
+int first = 1;
+
 void repeatMe() {
   auto imu_update = M5.Imu.update();
   if (imu_update) {
@@ -51,13 +56,20 @@ void repeatMe() {
 
     aranovskiy_update(&params, &state, y, delta_t);
 
+    if (first) {
+      kalman_smoother_set_initial(&kalman, state.f);
+      first = 0;
+    }
+    double freq_adj = kalman_smoother_update(&kalman, state.f);
+
     if (now - last_refresh >= 1000000) {
       M5.Lcd.setCursor(0, 10);
       M5.Lcd.clear();  // Delay 100ms
 
       M5.Lcd.printf("IMU:\n\n");
       M5.Lcd.printf("sec: %d\n\n", now / 1000000);
       M5.Lcd.printf("period sec: %0.4f\n\n", (state.f > 0 ? 1.0 / state.f : 9999.0));
+      M5.Lcd.printf("period adj: %0.4f\n\n", (freq_adj > 0 ? 1.0 / freq_adj : 9999.0));
       M5.Lcd.printf("samples: %d\n\n", got_samples);
       M5.Lcd.printf("%0.3f %0.3f %0.3f\n\n", accel.x, accel.y, accel.z - 1.0);
 
@@ -73,6 +85,7 @@ void setup(void) {
 
   aranovskiy_default_params(&params, omega_up, k_gain);
   aranovskiy_init_state(&state, t_0, x1_0, theta_0, sigma_0);
+  kalman_smoother_init(&kalman, 0.003, 5.0, 100.0);
 
   last_update = micros();
 }