From 4df6b2e6d8d15db35e2794abf54b2018e99f5b32 Mon Sep 17 00:00:00 2001 From: Mikhail Grushinskiy Date: Tue, 10 Sep 2024 21:45:04 -0400 Subject: [PATCH] Update KalmanForWave.h --- bbn_wave_freq_m5atomS3/KalmanForWave.h | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/bbn_wave_freq_m5atomS3/KalmanForWave.h b/bbn_wave_freq_m5atomS3/KalmanForWave.h index bfd74f4..ef0125b 100644 --- a/bbn_wave_freq_m5atomS3/KalmanForWave.h +++ b/bbn_wave_freq_m5atomS3/KalmanForWave.h @@ -165,17 +165,17 @@ void kalman_wave_init_defaults() { // transition covariance [KALMAN_NUM_STATES * KALMAN_NUM_STATES] matrix_t *Q = kalman_get_process_noise(kf); - matrix_data_t variance = (matrix_data_t) 2.0; + matrix_data_t variance = (matrix_data_t) 1.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.05 * variance); + matrix_set_symmetric(Q, 1, 1, (matrix_data_t)0.2 * 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.0025 * variance); + matrix_set_symmetric(Q, 2, 2, (matrix_data_t)0.04 * variance); matrix_set_symmetric(Q, 2, 3, (matrix_data_t)0.0); - matrix_set_symmetric(Q, 3, 3, (matrix_data_t)0.000125 * variance); + matrix_set_symmetric(Q, 3, 3, (matrix_data_t)0.008 * variance); } void kalman_wave_step(KalmanWaveState* state, float accel, float delta_t) {