From c328db9039f55bbc451360b4aea6f08c064e43d4 Mon Sep 17 00:00:00 2001 From: Mikhail Grushinskiy Date: Sat, 7 Sep 2024 15:09:04 -0400 Subject: [PATCH] Update Kalman.h --- bbn_wave_freq_m5atomS3/Kalman.h | 12 ++++++++++-- 1 file changed, 10 insertions(+), 2 deletions(-) diff --git a/bbn_wave_freq_m5atomS3/Kalman.h b/bbn_wave_freq_m5atomS3/Kalman.h index b3ae1ac..703eb9b 100644 --- a/bbn_wave_freq_m5atomS3/Kalman.h +++ b/bbn_wave_freq_m5atomS3/Kalman.h @@ -558,19 +558,27 @@ void kalman_init_process_noise(matrix_t *Q, float dt, float variance) { void kalman_predict_x(register kalman_t *const kf) { // matrices and vectors const matrix_t *const A = &kf->F; + const matrix_t *const B = &kf->B; matrix_t *const x = &kf->x; - + matrix_t *const u = &kf->u; + // temporaries matrix_t *const xpredicted = &kf->temporary.predicted_x; /************************************************************************/ /* Predict next state using system dynamics */ - /* x = A*x */ + /* x = A*x + B*u */ /************************************************************************/ // x = A*x matrix_mult_rowvector(A, x, xpredicted); matrix_copy(xpredicted, x); + + // x += B*u + if (kf->B.rows > 0) { + matrix_mult_rowvector(B, u, xpredicted); + matrix_add_inplace(x, xpredicted); + } } /*!