171 integrate ekf into the dotbot firmware #202
Conversation
Velocity is bidirectional, it needs an int variable, not a uint
There was a problem hiding this comment.
Thanks for the contribution, that should make the autonomous navigation more precise and reliable.
I don't have the impression that rebasing this PR on top of the latest main branch is so difficult. And it could be a good exercise for beginners.
Before trying to rebase, I'd like this PR to be cleaned up a bit. And I have some questions inline.
| */ | ||
| void EKF_set_P(KalmanFilter_t *ekf_p, float *P0); | ||
|
|
||
| //=========================== Private ========================================== |
There was a problem hiding this comment.
If all the functions below are private their declaration should be done in the .c file directly. Prefixing them with _ is just a convention and doesn't make them private if placed in a header.
| float P[25]; | ||
| float Q[5]; // For Q we keep only the diagonal | ||
| // The R and H matrices are stored in their respective update function. | ||
| } KalmanFilter_t; |
There was a problem hiding this comment.
The code convention (that I try to follow in this project) is lower snake case. So the structure should be renamed kalman_filter_t.
| * @param[in] x0 Initial state of the filter. [x, y, theta, V, W] | ||
| * @param[in] P0 Diagonal values of the initial Covariance Matrix. [Px, Py, Ptheta, Pv, Pw] | ||
| */ | ||
| void EKF_init(KalmanFilter_t *ekf_p, float *x0 ,float *P0); |
There was a problem hiding this comment.
Still about the lower snake case, all functions should be renamed ekf_
| float _EKF_angle_overflow(float rad); | ||
|
|
||
|
|
||
| #endif |
There was a problem hiding this comment.
There's a missing newline at the end of the file. I checked and there's no option to enable adding a newline automatically in SES...
| @@ -0,0 +1,481 @@ | |||
| #include <stdbool.h> | |||
There was a problem hiding this comment.
Copyright is missing
| #include <stdbool.h> | |
| /** | |
| * @file ekf.c | |
| * @author said <said@inria.fr> | |
| * @brief Extended Kalman Filter implementation | |
| * | |
| * @copyright Inria, 2023 | |
| * | |
| */ | |
| #include <stdbool.h> |
| //db_ism330_gyro_read(&_dotbot_vars.gyro); | ||
| //int32_t Z = (int32_t)(_dotbot_vars.gyro.z * 180 / M_PI * 1e3); | ||
| //_radio_ekf_debug_data(); |
There was a problem hiding this comment.
Remove if not necessary
| //db_ism330_gyro_read(&_dotbot_vars.gyro); | |
| //int32_t Z = (int32_t)(_dotbot_vars.gyro.z * 180 / M_PI * 1e3); | |
| //_radio_ekf_debug_data(); |
|
|
||
| //=========================== defines ========================================== | ||
|
|
||
| #define DB_LH2_UPDATE_DELAY_MS (100U) ///< 100ms delay between each LH2 data refresh | ||
| #define DB_ADVERTIZEMENT_DELAY_MS (500U) ///< 500ms delay between each advertizement packet sending | ||
| #define DB_TIMEOUT_CHECK_DELAY_MS (200U) ///< 200ms delay between each timeout delay check | ||
| #define DB_EKF_PREDICT_DELAY_US (20000U) ///< 20ms delay between each EKF prediction |
There was a problem hiding this comment.
You could just use ms directly
| #define DB_EKF_PREDICT_DELAY_US (20000U) ///< 20ms delay between each EKF prediction | |
| #define DB_EKF_PREDICT_DELAY_MS (20U) ///< 20ms delay between each EKF prediction |
|
|
||
| //=========================== defines ========================================== | ||
|
|
||
| #define DB_LH2_UPDATE_DELAY_MS (100U) ///< 100ms delay between each LH2 data refresh | ||
| #define DB_ADVERTIZEMENT_DELAY_MS (500U) ///< 500ms delay between each advertizement packet sending | ||
| #define DB_TIMEOUT_CHECK_DELAY_MS (200U) ///< 200ms delay between each timeout delay check | ||
| #define DB_EKF_PREDICT_DELAY_US (20000U) ///< 20ms delay between each EKF prediction |
There was a problem hiding this comment.
You could just use ms directly
| #define DB_EKF_PREDICT_DELAY_US (20000U) ///< 20ms delay between each EKF prediction | |
| #define DB_EKF_PREDICT_DELAY_MS (20U) ///< 20ms delay between each EKF prediction |
| db_timer_set_periodic_ms(0, DB_TIMEOUT_CHECK_DELAY_MS, &_timeout_check); | ||
| db_timer_set_periodic_ms(1, DB_ADVERTIZEMENT_DELAY_MS, &_advertise); | ||
| db_timer_set_periodic_ms(2, DB_LH2_UPDATE_DELAY_MS, &_update_lh2); | ||
| db_timer_hf_set_periodic_us(0, DB_EKF_PREDICT_DELAY_US, &_ekf_predict_and_gyro_flag_update); |
There was a problem hiding this comment.
| db_timer_hf_set_periodic_us(0, DB_EKF_PREDICT_DELAY_US, &_ekf_predict_and_gyro_flag_update); | |
| db_timer_hf_set_periodic_ms(0, DB_EKF_PREDICT_DELAY_US, &_ekf_predict_and_gyro_flag_update); |
| db_radio_rx_disable(); | ||
| db_radio_tx(_dotbot_vars.radio_buffer, length); | ||
| db_radio_rx_enable(); | ||
| } |
|
Another comment that's not added to the initial review: in the ekf drv implementation directory, it's very important to add a README.md file explaining as much as possible how the matrix multiplication code was created from python to C. And maybe also add the python script that can be used to generate this C code. |
|
|
||
| // RUN EKF UPDATE LH2 HERE | ||
| // ###################################################################### | ||
| // EKF assumes that the PyDotBot controller was calibrated with a 40x40cm box |
There was a problem hiding this comment.
| // EKF assumes that the PyDotBot controller was calibrated with a 40x40cm box | |
| // EKF assumes that the PyDotBot controller was calibrated with a 40x40cm box |
fixes #171