From 4d0b7858c2e2b32582c424afd97fd23996ed9545 Mon Sep 17 00:00:00 2001 From: Mikhail Grushinskiy Date: Fri, 6 Sep 2024 18:19:37 -0400 Subject: [PATCH] Update Kalman.h --- bbn_wave_freq_m5atomS3/Kalman.h | 27 +++++++++++++++++++++++++++ 1 file changed, 27 insertions(+) diff --git a/bbn_wave_freq_m5atomS3/Kalman.h b/bbn_wave_freq_m5atomS3/Kalman.h index a6e0daf..e27a7aa 100644 --- a/bbn_wave_freq_m5atomS3/Kalman.h +++ b/bbn_wave_freq_m5atomS3/Kalman.h @@ -311,6 +311,33 @@ inline void kalman_predict(kalman_t *kf) { kalman_predict_P(kf); } +/*! + * \brief Performs the time update / prediction step. + * \param[in] kf The Kalman Filter structure to predict with. + * \param[in] lambda Lambda factor (\c 0 < {\ref lambda} <= \c 1) to forcibly + * reduce prediction certainty. Smaller values mean larger uncertainty. + * + * This call assumes that the input covariance and variables are already set in + * the filter structure. + * + * \see kalman_predict_x + * \see kalman_predict_P_tuned + */ +void kalman_predict_tuned(kalman_t *kf, matrix_data_t lambda) { + /************************************************************************/ + /* Predict next state using system dynamics */ + /* x = F*x */ + /************************************************************************/ + + kalman_predict_x(kf); + + /************************************************************************/ + /* Predict next covariance using system dynamics and input */ + /* P = A*P*A' * 1/lambda^2 + B*Q*B' */ + /************************************************************************/ + + kalman_predict_P_tuned(kf, lambda); +} /*! * \brief Performs the measurement update step.