Update KalmanForWave.h

bareboat-necessities · Sep 10, 2024 · 95d80f4 · 95d80f4
1 parent 3653246
commit 95d80f4
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
@@ -158,16 +158,16 @@ void kalman_wave_init_defaults() {
 
   // transition covariance [KALMAN_NUM_STATES * KALMAN_NUM_STATES]
   matrix_t *Q = kalman_get_process_noise(kf);
-  matrix_set_symmetric(Q, 0, 0, (matrix_data_t)1.0);
+  matrix_set_symmetric(Q, 0, 0, (matrix_data_t)0.001);
   matrix_set_symmetric(Q, 0, 1, (matrix_data_t)0.0);
   matrix_set_symmetric(Q, 0, 2, (matrix_data_t)0.0);
   matrix_set_symmetric(Q, 0, 3, (matrix_data_t)0.0);
-  matrix_set_symmetric(Q, 1, 1, (matrix_data_t)0.2);
+  matrix_set_symmetric(Q, 1, 1, (matrix_data_t)0.1);
   matrix_set_symmetric(Q, 1, 2, (matrix_data_t)0.0);
   matrix_set_symmetric(Q, 1, 3, (matrix_data_t)0.0);
-  matrix_set_symmetric(Q, 2, 2, (matrix_data_t)0.1);
+  matrix_set_symmetric(Q, 2, 2, (matrix_data_t)1.0);
   matrix_set_symmetric(Q, 2, 3, (matrix_data_t)0.0);
-  matrix_set_symmetric(Q, 3, 3, (matrix_data_t)0.05);
+  matrix_set_symmetric(Q, 3, 3, (matrix_data_t)0.001);
 }
 
 void kalman_wave_step(KalmanWaveState* state, float accel, float delta_t) {