Merge branch 'humble-devel' into test-drones-again

Author: javizqh

CustomRobots/turtlebot3/models/turtlebot3/turtlebot3.urdf.xacro

@@ -6,25 +6,26 @@
   <xacro:arg name="noise_level" default="none"/>
   <xacro:property name="noise_level" value="$(arg noise_level)"/>
 
+  <!-- Odometry noise per difficulty (drift std ~ alpha*sqrt(distance)). -->
   <xacro:if value="${noise_level == 'low'}">
-    <xacro:property name="a1" value="0.1" />
-    <xacro:property name="a2" value="0.1" />
-    <xacro:property name="a3" value="0.05" />
-    <xacro:property name="a4" value="0.05" />
+    <xacro:property name="a1" value="0.01" />
+    <xacro:property name="a2" value="0.01" />
+    <xacro:property name="a3" value="0.01" />
+    <xacro:property name="a4" value="0.01" />
   </xacro:if>
 
   <xacro:if value="${noise_level == 'med'}">
-    <xacro:property name="a1" value="0.5" />
-    <xacro:property name="a2" value="0.5" />
-    <xacro:property name="a3" value="0.25" />
-    <xacro:property name="a4" value="0.25" />
+    <xacro:property name="a1" value="0.05" />
+    <xacro:property name="a2" value="0.05" />
+    <xacro:property name="a3" value="0.05" />
+    <xacro:property name="a4" value="0.05" />
   </xacro:if>
 
   <xacro:if value="${noise_level == 'high'}">
-    <xacro:property name="a1" value="1.5" />
-    <xacro:property name="a2" value="1.5" />
-    <xacro:property name="a3" value="0.8" />
-    <xacro:property name="a4" value="0.8" />
+    <xacro:property name="a1" value="0.25" />
+    <xacro:property name="a2" value="0.25" />
+    <xacro:property name="a3" value="0.25" />
+    <xacro:property name="a4" value="0.25" />
   </xacro:if>
 
   <xacro:include filename="$(find custom_robots)/models/turtlebot3/turtlebot3_gz.urdf.xacro"/>

Industrial/gz_noisy_odometry/src/gz_noisy_odometry/NoisyOdometryPlugin.cpp

@@ -127,11 +127,10 @@ namespace custom_plugins
     const double ds     = wheel_radius_ * (d_right + d_left)  / 2.0;
     const double dtheta = wheel_radius_ * (d_right - d_left) / wheel_separation_;
 
-    // Odometry motion-model noise (Probabilistic Robotics, Ch. 5).
-    // Translation error grows with distance and heading change; rotation error
-    // grows with heading change and distance — both accumulate over time.
-    const double sigma_trans = alpha3_ * std::abs(ds)     + alpha4_ * std::abs(dtheta);
-    const double sigma_rot   = alpha1_ * std::abs(dtheta) + alpha2_ * std::abs(ds);
+    // Motion-model noise: per-tick std scales with sqrt(increment), so drift
+    // variance grows linearly with travelled distance (step-rate independent).
+    const double sigma_trans = alpha3_ * std::sqrt(std::abs(ds))     + alpha4_ * std::sqrt(std::abs(dtheta));
+    const double sigma_rot   = alpha1_ * std::sqrt(std::abs(dtheta)) + alpha2_ * std::sqrt(std::abs(ds));
 
     const double ds_noisy     = ds     + gz::math::Rand::DblNormal(0.0, sigma_trans);
     const double dtheta_noisy = dtheta + gz::math::Rand::DblNormal(0.0, sigma_rot);

database/universes.sql

@@ -310,4 +310,4 @@ CREATE INDEX exercises_universe_name_459df99a_like ON public.universes USING btr
 
 --
 -- PostgreSQL database dump complete
---
+--