From 1b2ce3787a9e8170132f6fffadec0ad6bb52cfa7 Mon Sep 17 00:00:00 2001 From: Mikhail Grushinskiy Date: Tue, 10 Sep 2024 21:33:34 -0400 Subject: [PATCH] Update KalmanForWave.h --- bbn_wave_freq_m5atomS3/KalmanForWave.h | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/bbn_wave_freq_m5atomS3/KalmanForWave.h b/bbn_wave_freq_m5atomS3/KalmanForWave.h index d51c4c3..bfd74f4 100644 --- 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) {