From 8253a4f6349618434371f8f5810cd1d98b500ec8 Mon Sep 17 00:00:00 2001
From: Mikhail Grushinskiy <mgrouch@users.noreply.github.com>
Date: Thu, 19 Sep 2024 18:39:41 -0400
Subject: [PATCH] Update bbn_wave_freq_m5atomS3.ino

---
 bbn_wave_freq_m5atomS3/bbn_wave_freq_m5atomS3.ino | 11 ++++++-----
 1 file changed, 6 insertions(+), 5 deletions(-)

diff --git a/bbn_wave_freq_m5atomS3/bbn_wave_freq_m5atomS3.ino b/bbn_wave_freq_m5atomS3/bbn_wave_freq_m5atomS3.ino
index f332063..b173919 100644
--- a/bbn_wave_freq_m5atomS3/bbn_wave_freq_m5atomS3.ino
+++ b/bbn_wave_freq_m5atomS3/bbn_wave_freq_m5atomS3.ino
@@ -74,7 +74,7 @@ static int prev_xpos[18];
 
 unsigned long now = 0UL, last_refresh = 0UL, last_update = 0UL, last_update_k = 0UL;
 unsigned long got_samples = 0;
-int first = 1;
+int first = 1, kalman_k_first = 1;
 
 float delta_t;  // time step sec
 
@@ -215,14 +215,15 @@ void repeatMe() {
         min_max_lemire_update(&min_max_h, sample, windowMicros);
 
         if (fabs(freq - freq_adj) < 0.3 * freq_adj) { /* sanity check of convergence for freq */
-          if (waveAltState.heave == 0.0f && waveAltState.vert_speed == 0.0f && waveAltState.vert_accel == 0.0f) {
-            waveAltState.heave = heave;
+          float k_hat = - pow(2.0 * PI * freq_adj, 2);
+          if (kalman_k_first) {
+            kalman_k_first = 0;
+            waveAltState.heave = waveState.heave;
             waveAltState.vert_speed = waveState.vert_speed;
-            waveAltState.vert_accel = a * g_std;
+            waveAltState.vert_accel = k_hat * waveState.heave; //a * g_std;
             waveAltState.accel_bias = 0.0f;
             kalman_wave_alt_init_state(&waveAltState);
           }
-          float k_hat = - pow(2.0 * PI * freq_adj, 2);
           float delta_t_k = last_update_k == 0UL ? delta_t : (now - last_update_k) / 1000000.0;
           kalman_wave_alt_step(&waveAltState, a * g_std, k_hat, delta_t_k);
           last_update_k = now;