From 3d203f1a9afd06aeb14d62f1f264e5cc20ac1d22 Mon Sep 17 00:00:00 2001 From: Mikhail Grushinskiy Date: Sat, 7 Sep 2024 13:57:17 -0400 Subject: [PATCH] Update KalmanForWave.h --- bbn_wave_freq_m5atomS3/KalmanForWave.h | 14 ++++++-------- 1 file changed, 6 insertions(+), 8 deletions(-) diff --git a/bbn_wave_freq_m5atomS3/KalmanForWave.h b/bbn_wave_freq_m5atomS3/KalmanForWave.h index 116fb0b..2630fd4 100644 --- a/bbn_wave_freq_m5atomS3/KalmanForWave.h +++ b/bbn_wave_freq_m5atomS3/KalmanForWave.h @@ -35,12 +35,12 @@ matrix_t *kalman_wave_get_state_transition(kalman_t *kf, matrix_data_t delta_t) return F; } -matrix_t *kalman_wave_get_transition_offset(kalman_t *kf, matrix_data_t delta_t) { +matrix_t *kalman_wave_get_transition_offset(kalman_t *kf, matrix_data_t delta_t, matrix_data_t accel) { // transition offset [KALMAN_NUM_STATES * KALMAN_NUM_INPUTS] matrix_t *B = kalman_get_input_transition(kf); - matrix_set(B, 0, 0, (matrix_data_t)(1.0 / 6.0) * delta_t * delta_t * delta_t); - matrix_set(B, 1, 0, (matrix_data_t)0.5 * delta_t * delta_t); - matrix_set(B, 2, 0, (matrix_data_t)delta_t); + matrix_set(B, 0, 0, (matrix_data_t)(1.0 / 6.0) * delta_t * delta_t * delta_t * accel); + matrix_set(B, 1, 0, (matrix_data_t)0.5 * delta_t * delta_t * accel); + matrix_set(B, 2, 0, (matrix_data_t)delta_t * accel); return B; } @@ -84,7 +84,7 @@ void kalman_wave_init() { matrix_set_symmetric(Q, 2, 2, (matrix_data_t)0.1); } -void kalman_wave_update(float accel, float delta_t) { +void kalman_wave_step(float accel, float delta_t) { kalman_t *kf = &kalman_filter_wave; kalman_measurement_t *kfm = &kalman_filter_wave_measurement_displacement_integral; @@ -92,9 +92,7 @@ void kalman_wave_update(float accel, float delta_t) { matrix_t *z = kalman_get_measurement_vector(kfm); matrix_t *F = kalman_wave_get_state_transition(kf, delta_t); - matrix_t *B = kalman_wave_get_transition_offset(kf, delta_t); - - // TODO: B = B * accel ? + matrix_t *B = kalman_wave_get_transition_offset(kf, delta_t, accel); // prediction. kalman_predict(kf);