diff --git a/bbn_wave_freq_m5atomS3/tests/tests.cpp b/bbn_wave_freq_m5atomS3/tests/tests.cpp
index 27f924c..15c8222 100644
--- a/bbn_wave_freq_m5atomS3/tests/tests.cpp
+++ b/bbn_wave_freq_m5atomS3/tests/tests.cpp
@@ -69,6 +69,7 @@ void run_fiters(float a, float v, float h, float delta_t) {
       float k_hat = - pow(2.0 * PI * freq_adj, 2);
       if (kalman_k_first) {
         kalman_k_first = false;
+        waveAltState.displacement_integral = 0.0f;
         waveAltState.heave = waveState.heave;
         waveAltState.vert_speed = waveState.vert_speed;
         waveAltState.vert_accel = k_hat * waveState.heave; //a * g_std;