From dac631c65d0d98a7edd598d5eddb8a487e8a896a Mon Sep 17 00:00:00 2001 From: Mikhail Grushinskiy Date: Wed, 11 Sep 2024 12:37:08 -0400 Subject: [PATCH] Update Kalman.h --- bbn_wave_freq_m5atomS3/Kalman.h | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/bbn_wave_freq_m5atomS3/Kalman.h b/bbn_wave_freq_m5atomS3/Kalman.h index d035a94..dcf57ec 100644 --- a/bbn_wave_freq_m5atomS3/Kalman.h +++ b/bbn_wave_freq_m5atomS3/Kalman.h @@ -298,14 +298,14 @@ void kalman_predict_P_tuned(register kalman_t *const kf, inline void kalman_predict(kalman_t *kf) { /************************************************************************/ /* Predict next state using system dynamics */ - /* x = F*x */ + /* x = F*x + B*u + w */ /************************************************************************/ kalman_predict_x(kf); /************************************************************************/ /* Predict next covariance using system dynamics and input */ - /* P = A*P*A' + Q */ + /* P = F*P*F' + Q */ /************************************************************************/ kalman_predict_P(kf); @@ -326,14 +326,14 @@ inline void kalman_predict(kalman_t *kf) { void kalman_predict_tuned(kalman_t *kf, matrix_data_t lambda) { /************************************************************************/ /* Predict next state using system dynamics */ - /* x = F*x */ + /* x = F*x + B*u + w */ /************************************************************************/ kalman_predict_x(kf); /************************************************************************/ /* Predict next covariance using system dynamics and input */ - /* P = A*P*A' * 1/lambda^2 + B*Q*B' */ + /* P = F*P*F' * 1/lambda^2 + B*Q*B' */ /************************************************************************/ kalman_predict_P_tuned(kf, lambda); @@ -567,7 +567,7 @@ void kalman_predict_x(register kalman_t *const kf) { /************************************************************************/ /* Predict next state using system dynamics */ - /* x = A*x + B*u */ + /* x = F*x + B*u + w */ /************************************************************************/ // x = A*x