From 8298d00f3222d46afe6bbb1b961307abe4d150ab Mon Sep 17 00:00:00 2001 From: Mikhail Grushinskiy Date: Wed, 11 Sep 2024 20:39:02 -0400 Subject: [PATCH] Update KalmanForWaveAlt.h --- bbn_wave_freq_m5atomS3/KalmanForWaveAlt.h | 17 +---------------- 1 file changed, 1 insertion(+), 16 deletions(-) diff --git a/bbn_wave_freq_m5atomS3/KalmanForWaveAlt.h b/bbn_wave_freq_m5atomS3/KalmanForWaveAlt.h index 5ca7190..4337ec4 100644 --- a/bbn_wave_freq_m5atomS3/KalmanForWaveAlt.h +++ b/bbn_wave_freq_m5atomS3/KalmanForWaveAlt.h @@ -108,7 +108,7 @@ matrix_t *kalman_wave_alt_get_state_transition(kalman_t *kf, matrix_data_t k_hat matrix_set(F, 1, 2, (matrix_data_t)delta_t); // T matrix_set(F, 1, 3, (matrix_data_t)-delta_t); // -T - matrix_set(F, 2, 0, (matrix_data_t)k_hat); // k + matrix_set(F, 2, 0, (matrix_data_t)k_hat); // k_hat matrix_set(F, 2, 1, (matrix_data_t)0.0); // 0 matrix_set(F, 2, 2, (matrix_data_t)0.0); // 0 matrix_set(F, 2, 3, (matrix_data_t)0.0); // 0 @@ -171,16 +171,6 @@ void kalman_wave_alt_init_defaults() { matrix_set_symmetric(Q, 3, 3, (matrix_data_t)0.008 * variance); } -matrix_t *kalman_wave_alt_get_transition_offset(kalman_t *kf, matrix_data_t delta_t) { - // transition offset [KALMAN_NUM_STATES * KALMAN_NUM_INPUTS] - matrix_t *B = kalman_get_input_transition(kf); - matrix_set(B, 0, 0, (matrix_data_t)0.0); - matrix_set(B, 1, 0, (matrix_data_t)0.0); - matrix_set(B, 2, 0, (matrix_data_t)0.0); - matrix_set(B, 3, 0, (matrix_data_t)0.0); - return B; -} - void kalman_wave_alt_step(KalmanWaveAltState* state, float accel, float k_hat, float delta_t) { kalman_t *kf = &kalman_filter_wave_alt; kalman_measurement_t *kfm = &kalman_filter_wave_alt_measurement_vert_accel; @@ -189,11 +179,6 @@ void kalman_wave_alt_step(KalmanWaveAltState* state, float accel, float k_hat, f matrix_t *z = kalman_get_measurement_vector(kfm); matrix_t *F = kalman_wave_alt_get_state_transition(kf, k_hat, delta_t); - matrix_t *B = kalman_wave_alt_get_transition_offset(kf, delta_t); - - // input vector [KALMAN_NUM_INPUTS * 1] - matrix_t *u = kalman_get_input_vector(kf); - matrix_set(u, 0, 0, 0.0); // prediction. kalman_predict(kf);