171 integrate ekf into the dotbot firmware by SaidAlvarado · Pull Request #202 · DotBots/DotBot-firmware

SaidAlvarado · 2023-05-12T09:36:59Z

fixes #171

Velocity is bidirectional, it needs an int variable, not a uint

aabadie

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.

aabadie · 2023-05-12T13:49:37Z

+ */
+void EKF_set_P(KalmanFilter_t *ekf_p, float *P0);
+
+//=========================== Private ==========================================


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.

aabadie · 2023-05-12T13:50:28Z

+    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;


The code convention (that I try to follow in this project) is lower snake case. So the structure should be renamed kalman_filter_t.

aabadie · 2023-05-12T13:51:55Z

+ * @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);


Still about the lower snake case, all functions should be renamed ekf_

aabadie · 2023-05-12T13:56:35Z

+float _EKF_angle_overflow(float rad);
+
+
+#endif


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...

aabadie · 2023-05-12T13:57:45Z

@@ -0,0 +1,481 @@
+#include <stdbool.h>


Copyright is missing

Suggested change

#include <stdbool.h>

/**

* @file ekf.c

* @author said <said@inria.fr>

* @brief Extended Kalman Filter implementation

*

* @copyright Inria, 2023

*

*/

#include <stdbool.h>

aabadie · 2023-05-12T14:27:49Z

+        //db_ism330_gyro_read(&_dotbot_vars.gyro);
+        //int32_t Z = (int32_t)(_dotbot_vars.gyro.z * 180 / M_PI * 1e3);
+        //_radio_ekf_debug_data();


Remove if not necessary

Suggested change

//db_ism330_gyro_read(&_dotbot_vars.gyro);

//int32_t Z = (int32_t)(_dotbot_vars.gyro.z * 180 / M_PI * 1e3);

//_radio_ekf_debug_data();

aabadie · 2023-05-12T14:29:29Z

 #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


You could just use ms directly

Suggested change

#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

aabadie · 2023-05-12T14:29:37Z

 #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


You could just use ms directly

Suggested change

#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

aabadie · 2023-05-12T14:29:50Z

    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);


Suggested change

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);

aabadie · 2023-05-12T14:30:11Z

+    db_radio_rx_disable();
+    db_radio_tx(_dotbot_vars.radio_buffer, length);
+    db_radio_rx_enable();
+}


Missing newline

aabadie · 2023-05-12T14:49:25Z

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.

aabadie · 2023-05-15T14:28:59Z

+
+                // RUN EKF UPDATE LH2 HERE
+                // ######################################################################
+                // EKF assumes that the PyDotBot controller was calibrated  with a 40x40cm box


Suggested change

// EKF assumes that the PyDotBot controller was calibrated with a 40x40cm box

// EKF assumes that the PyDotBot controller was calibrated with a 40x40cm box

geonnave · 2026-09-25T12:09:29Z

Thanks for this, @SaidAlvarado. Closing it as superseded: pose estimation now lives in drv/pose_estimator in DotBot-libs, and the rebuilt sandbox dotbot app (#425 to #429) uses it. If parts of this EKF are still worth bringing over, a fresh PR on current main would be welcome.

SaidAlvarado added 3 commits March 31, 2023 17:35

feat: Added ekf code

8d33dcf

feat: add protocol message to send ekf information for debug.

35fd1db

fix comm protocol

0fc7d8a

Velocity is bidirectional, it needs an int variable, not a uint

SaidAlvarado requested a review from aabadie May 12, 2023 09:36

SaidAlvarado linked an issue May 12, 2023 that may be closed by this pull request

Integrate EKF into the DotBot firmware #171

Open

SaidAlvarado mentioned this pull request May 12, 2023

90 add support to ekf debug radio protocol messages DotBots/PyDotBot#95

Closed

aabadie requested changes May 12, 2023

View reviewed changes

aabadie reviewed May 15, 2023

View reviewed changes

aabadie added the new feature New feature label May 15, 2023

geonnave closed this Sep 25, 2026

		float _EKF_angle_overflow(float rad);


		#endif No newline at end of file

-#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();

	#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_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);

	// EKF assumes that the PyDotBot controller was calibrated with a 40x40cm box
	// EKF assumes that the PyDotBot controller was calibrated with a 40x40cm box

Conversation

SaidAlvarado commented May 12, 2023 • edited by aabadie Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Uh oh!

aabadie left a comment • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

aabadie commented May 12, 2023 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Uh oh!

Choose a reason for hiding this comment

Uh oh!

geonnave commented Sep 25, 2026 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Uh oh!

Reviewers

Assignees

Labels

Projects

Milestone

Development

Uh oh!

3 participants

SaidAlvarado commented May 12, 2023 •

edited by aabadie

Loading

aabadie left a comment •

edited

Loading

aabadie commented May 12, 2023 •

edited

Loading

geonnave commented Sep 25, 2026 •

edited

Loading