Update KalmanForWave.h

bareboat-necessities · Sep 22, 2024 · 701ed49 · 701ed49
1 parent f2398a8
commit 701ed49
Showing 1 changed file with 4 additions and 4 deletions.
diff --git a/bbn_wave_freq_m5atomS3/KalmanForWave.h b/bbn_wave_freq_m5atomS3/KalmanForWave.h
@@ -132,10 +132,10 @@ void kalman_wave_init_state(KalmanWaveState* state) {
 
   // [KALMAN_NUM_STATES * 1]
   matrix_t *x = kalman_get_state_vector(kf);
-  x->data[1] = state->displacement_integral; // displacement integral
-  x->data[2] = state->heave;                 // vertical displacement
-  x->data[3] = state->vert_speed;            // vertical velocity
-  x->data[4] = state->accel_bias;            // accel bias  
+  x->data[0] = state->displacement_integral; // displacement integral
+  x->data[1] = state->heave;                 // vertical displacement
+  x->data[2] = state->vert_speed;            // vertical velocity
+  x->data[3] = state->accel_bias;            // accel bias  
 }
 
 void kalman_wave_init_defaults() {