Update KalmanForWave.h

bareboat-necessities · Sep 11, 2024 · 1b2ce37 · 1b2ce37
1 parent 7e459d1
commit 1b2ce37
Showing 1 changed file with 5 additions and 4 deletions.
diff --git a/bbn_wave_freq_m5atomS3/KalmanForWave.h b/bbn_wave_freq_m5atomS3/KalmanForWave.h
@@ -165,16 +165,17 @@ 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_data_t variance = (matrix_data_t) 2.0;
+  matrix_set_symmetric(Q, 0, 0, (matrix_data_t)variance);
   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.05 * variance);
   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.04);
+  matrix_set_symmetric(Q, 2, 2, (matrix_data_t)0.0025 * variance);
   matrix_set_symmetric(Q, 2, 3, (matrix_data_t)0.0);
-  matrix_set_symmetric(Q, 3, 3, (matrix_data_t)0.008);
+  matrix_set_symmetric(Q, 3, 3, (matrix_data_t)0.000125 * variance);
 }
 
 void kalman_wave_step(KalmanWaveState* state, float accel, float delta_t) {