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) {