Feature/sensor fusion.ekf by haecto · Pull Request #208 · BlueAndi/DroidControlShip

haecto · 2025-12-08T13:26:08Z

Added:
EKF
TimeSync Adapted

…F_jacobian

lib/APPLineFollowerSensorFusion/src/App.cpp

jkerpe · 2025-12-08T15:42:16Z

lib/APPLineFollowerSensorFusion/src/App.cpp

-    const bool     rtcSynced     = m_timeSync.isRtcSynced();
+    const bool     hostSynced    = m_timeSync.isHostSynced();
+
+    (void)offsetMs; // keep for future debug if not used directly


Please always use /* ... */ comments.

Still used in Line 369.

lib/APPLineFollowerSensorFusion/src/App.cpp

lib/APPLineFollowerSensorFusion/src/EKF.cpp

lib/APPLineFollowerSensorFusion/src/TimeSync.cpp

jkerpe · 2025-12-09T07:48:30Z

data/config/config_zumo1.json

@@ -1,5 +1,5 @@
 {
-  "robotName": "1",
+  "robotName": "0",


"zumo1" has the name "0"? does not match with the other config files.

the RobotName defined here is the topic or aruco marcer number and has to match the one provided by SSR

Then rename the file. It's confusing that robotName does not match the filename.

lib/APPLineFollowerSensorFusion/src/EKF.cpp

jkerpe · 2025-12-09T08:10:04Z

lib/APPLineFollowerSensorFusion/src/TimeSync.cpp

+    const int64_t rtt = (static_cast<int64_t>(t4EspMs) - static_cast<int64_t>(t1EspMs))
+                      - (static_cast<int64_t>(t3HostMs) - static_cast<int64_t>(t2HostMs));


Can't you use int64_t in the first place?

you mean because of the repetitive casts?

jkerpe · 2025-12-09T08:10:58Z

lib/APPLineFollowerSensorFusion/src/TimeSync.cpp

+    m_lastHostRttMs = (rtt >= 0) ? static_cast<uint32_t>(rtt) : 0U;
+    m_hostSyncValid = true;
+
+    const double offset_s = static_cast<double>(m_hostOffsetMs) / 1000.0;


Can't you use float here?

I'm not 100% sure, but i will try

... and did it work?

.env

lib/APPLineFollowerSensorFusion/src/App.cpp

gabryelreyes · 2025-12-15T21:11:52Z

start_sensorFusion.ps1

+        "--supervisorTxCh", "2",
+        "--serialRxCh",     "3",
+        "--serialTxCh",     "4",
+        "--settingsPath",   "C:\Users\thaeckel\Documents\Repos\RadonUlzer\./settings/settings.json",


Try to use relative paths. Another developer will not be able to use this script

gabryelreyes · 2025-12-15T21:12:35Z

start_sensorFusion.ps1

+$webotsController = Join-Path $env:WEBOTS_HOME "msys64\mingw64\bin\webots-controller.exe"
+
+# ===== RadonUlzer (Zumo / RemoteControlSim) ==================================
+# Entspricht deinem PIO-Build-Aufruf:


Please use english as the standard language for all files that are commited to the repository

jkerpe · 2025-12-19T07:21:35Z

data/config/config_zumo1.json

@@ -1,5 +1,5 @@
 {
-  "robotName": "1",
+  "robotName": "0",


Then rename the file. It's confusing that robotName does not match the filename.

jkerpe · 2025-12-19T07:22:34Z

lib/APPLineFollowerSensorFusion/src/EKF.cpp

+    x_next(0) = px + v * std::cos(thetaRad) * dt; /* p_x */
+    x_next(1) = py + v * std::sin(thetaRad) * dt; /* p_y */
+    x_next(2) = thetaMrad + omegaMrads * dt;       /* theta [mrad] */
+    x_next(3) = v + a_x_mms * dt;                 /* v */
+    x_next(4) = omegaMrads;                        /* omega */


Please fix indentation.

jkerpe · 2025-12-19T07:24:16Z

lib/APPLineFollowerSensorFusion/src/TimeSync.cpp

+    const int64_t rtt = (static_cast<int64_t>(t4EspMs) - static_cast<int64_t>(t1EspMs))
+                      - (static_cast<int64_t>(t3HostMs) - static_cast<int64_t>(t2HostMs));


jkerpe · 2025-12-19T07:25:43Z

lib/APPLineFollowerSensorFusion/src/TimeSync.cpp

+    m_lastHostRttMs = (rtt >= 0) ? static_cast<uint32_t>(rtt) : 0U;
+    m_hostSyncValid = true;
+
+    const double offset_s = static_cast<double>(m_hostOffsetMs) / 1000.0;


... and did it work?

…ted comment from // to /**/

gabryelreyes

There are still open review findings

Tobias Haeckel added 4 commits December 4, 2025 16:31

first implement of EKF

ec5ce26

added Startup Script

ec65580

Working Fusion, but Vehicle Data is Dominating over SSR

664087b

Working TimeSync now

4661eb0

haecto requested review from gabryelreyes and jkerpe December 8, 2025 13:26

Tobias Haeckel added 2 commits December 8, 2025 14:35

Added Doxygen Comments and changed From StateMatrix F to StateMatrix …

cb30c0e

…F_jacobian

added Dooxygen comments

c1348ca

jkerpe requested changes Dec 9, 2025

View reviewed changes

gabryelreyes requested changes Dec 11, 2025

View reviewed changes

Tobias Haeckel added 6 commits December 11, 2025 16:54

Applied changes from Review

50811c6

Changed some Variable names, so it is consitent now

70a4c21

added doxygen comment

a70d80e

More Doxygen

cd20ffe

And even more doxygen

955fcb6

Updated EKF Model

a7a8683

gabryelreyes requested changes Dec 15, 2025

View reviewed changes

Rebuild Startup Script

e679a2c

haecto requested review from gabryelreyes and jkerpe December 18, 2025 12:08

jkerpe reviewed Dec 19, 2025

View reviewed changes

Changed File Names according to Robot Names, Startup Script and adjus…

28650bd

…ted comment from // to /**/

gabryelreyes requested changes Jan 5, 2026

View reviewed changes

		const int64_t rtt = (static_cast<int64_t>(t4EspMs) - static_cast<int64_t>(t1EspMs))
		- (static_cast<int64_t>(t3HostMs) - static_cast<int64_t>(t2HostMs));

Conversation

haecto commented Dec 8, 2025

Uh oh!

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!

Uh oh!

Uh oh!

Uh oh!

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!

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!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

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!

gabryelreyes left a comment

Choose a reason for hiding this comment

Uh oh!

Reviewers

Assignees

Labels

Projects

Milestone

Development

Uh oh!

3 participants