From e222b33f8ae79cf3c53c3daa3b1002f9ccd015a9 Mon Sep 17 00:00:00 2001 From: Mikhail Grushinskiy Date: Thu, 12 Sep 2024 10:26:04 -0400 Subject: [PATCH] Update KalmanForWaveAlt.h --- bbn_wave_freq_m5atomS3/KalmanForWaveAlt.h | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/bbn_wave_freq_m5atomS3/KalmanForWaveAlt.h b/bbn_wave_freq_m5atomS3/KalmanForWaveAlt.h index e03fc86..f7ef205 100644 --- a/bbn_wave_freq_m5atomS3/KalmanForWaveAlt.h +++ b/bbn_wave_freq_m5atomS3/KalmanForWaveAlt.h @@ -166,9 +166,9 @@ void kalman_wave_alt_init_defaults() { // observation covariance [KALMAN_NUM_MEASUREMENTS * KALMAN_NUM_MEASUREMENTS] matrix_t *R = kalman_get_observation_noise(kfm); - matrix_set_symmetric(R, 0, 0, (matrix_data_t)1.0); + matrix_set_symmetric(R, 0, 0, (matrix_data_t)0.01); matrix_set_symmetric(R, 0, 1, (matrix_data_t)0.0); - matrix_set_symmetric(R, 1, 1, (matrix_data_t)1.0); + matrix_set_symmetric(R, 1, 1, (matrix_data_t)0.01); // initial state covariance [KALMAN_NUM_STATES * KALMAN_NUM_STATES] matrix_t *P = kalman_get_system_covariance(kf); @@ -203,7 +203,7 @@ void kalman_wave_alt_init_defaults() { 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, 2, 4, (matrix_data_t)0.0); - matrix_set_symmetric(Q, 3, 3, (matrix_data_t)variance); + matrix_set_symmetric(Q, 3, 3, (matrix_data_t)0.008 * variance); matrix_set_symmetric(Q, 3, 4, (matrix_data_t)0.0); matrix_set_symmetric(Q, 4, 4, (matrix_data_t)0.0002 * variance); }