Improved odometry

.github/workflows/main.yml

@@ -22,10 +22,10 @@ jobs:
     - run: echo "The name of the branch is ${{ github.ref }} and the repository is ${{ github.repository }}."
 
     - name: Checkout repository
-      uses: actions/checkout@v4
+      uses: actions/checkout@v7
 
     - name: Set up Python
-      uses: actions/setup-python@v5
+      uses: actions/setup-python@v6
       with:
         python-version: '3.10'
         cache: 'pip'
@@ -48,10 +48,10 @@ jobs:
     # Steps represent a sequence of tasks that will be executed as part of the job.
     steps:
     - name: Checkout repository
-      uses: actions/checkout@v4
+      uses: actions/checkout@v7
 
     - name: Set up Python
-      uses: actions/setup-python@v5
+      uses: actions/setup-python@v6
       with:
         python-version: '3.10'
         cache: 'pip'
@@ -80,10 +80,10 @@ jobs:
 
     steps:
     - name: Checkout repository
-      uses: actions/checkout@v4
+      uses: actions/checkout@v7
 
     - name: Set up Python
-      uses: actions/setup-python@v5
+      uses: actions/setup-python@v6
       with:
         python-version: '3.10'
         cache: 'pip'
@@ -108,10 +108,10 @@ jobs:
 
     steps:
     - name: Checkout repository
-      uses: actions/checkout@v4
+      uses: actions/checkout@v7
 
     - name: Set up Python
-      uses: actions/setup-python@v5
+      uses: actions/setup-python@v6
       with:
         python-version: '3.10'
         cache: 'pip'
@@ -135,10 +135,10 @@ jobs:
 
     steps:
     - name: Checkout repository
-      uses: actions/checkout@v4
+      uses: actions/checkout@v7
 
     - name: Set up Python
-      uses: actions/setup-python@v5
+      uses: actions/setup-python@v6
       with:
         python-version: '3.10'
         cache: 'pip'
@@ -170,10 +170,10 @@ jobs:
     # Steps represent a sequence of tasks that will be executed as part of the job.
     steps:
       - name: Checkout repository
-        uses: actions/checkout@v4
+        uses: actions/checkout@v7
 
       - name: Set up Python
-        uses: actions/setup-python@v5
+        uses: actions/setup-python@v6
         with:
           python-version: '3.10'
           cache: 'pip'

doc/architecture/README.md

@@ -91,10 +91,10 @@ $v_R [\frac{steps}{s}] = \frac{w_r [\frac{rad}{s}] \cdot W [mm]}{2} \cdot \frac{
 
 Consider robot linear speed center\
 Linear speed left\
-$v_L [\frac{steps}{s}] = \frac{v_{Linear}}{2} [\frac{steps}{s}] -\frac{w_r [\frac{rad}{s}] \cdot W [mm]}{2} \cdot \frac{ENC [\frac{steps}{m}]}{1000}$
+$v_L [\frac{steps}{s}] = v_{Linear} [\frac{steps}{s}] -\frac{w_r [\frac{rad}{s}] \cdot W [mm]}{2} \cdot \frac{ENC [\frac{steps}{m}]}{1000}$
 
 Linear speed right\
-$v_R [\frac{steps}{s}] = \frac{v_{Linear}}{2} [\frac{steps}{s}] + \frac{w_r [\frac{rad}{s}] \cdot W [mm]}{2} \cdot \frac{ENC [\frac{steps}{m}]}{1000}$
+$v_R [\frac{steps}{s}] = v_{Linear} [\frac{steps}{s}] + \frac{w_r [\frac{rad}{s}] \cdot W [mm]}{2} \cdot \frac{ENC [\frac{steps}{m}]}{1000}$
 
 Consider angular speed in mrad per s\
 Linear speed left\
@@ -111,31 +111,57 @@ Base equations:
 
 - $distanceLeft [mm] = \frac{encoderStepsLeft [steps]}{encoderStepsPerMM [\frac{steps}{mm}]}$
 - $distanceRight [mm] = \frac{encoderStepsRight [steps]}{encoderStepsPerMM [\frac{steps}{mm}]}$
-- $stepsCenter [steps] = \frac{encoderStepsLeft - encoderStepsRight}{2}$
+- $stepsCenter [steps] = \frac{encoderStepsLeft + encoderStepsRight}{2}$
 - $distanceCenter [mm] = \frac{stepsCenter [steps]}{encoderStepsPerMM [\frac{steps}{mm}]}$
 
 Orientation:
 
 - $alpha [rad] = \frac{distanceRight [mm] - distanceLeft [mm]}{wheelBase [mm]}$
-- $orientation' [rad] = orientation [rad] + alpha [rad]$
-- $orientation' [rad] = orientation [rad]~\%~2\pi$
-- $-2\pi < Orientation < 2\pi$
+- $orientation_{new} [rad] = orientation_{old} [rad] + alpha [rad]$
+- $orientation_{new} [rad] = orientation_{new} [rad]~%~2\pi$
+- $-2\pi < orientation < 2\pi$
 - After wrapping on the positive limit $2\pi$, the orientation remains positive and starts from 0 again.
 - After wrapping on the negative limit $2\pi$, the orientation remains negative and starts from 0 again.
 
 Position:
 
-- $dX [mm] = -distanceCenter [mm] \cdot sin(orientation' [rad])$ <- Approximation for performance reason
-- $dY [mm] = distanceCenter [mm] \cdot cos(orientation' [rad])$ <- Approximation for performance reason
-- $x' [mm] = x [mm] + dX [mm]$
-- $y' [mm] = y [mm] + dY [mm]$
+To reduce the integration error during curved movement, the position is calculated using the orientation in the middle of the movement interval.
 
-Improvement for better accuracy:
+- $orientation_{mid} [rad] = orientation_{old} [rad] + \frac{alpha [rad]}{2}$
+
+Position update:
+
+- $dX [mm] = -distanceCenter [mm] \cdot sin(orientation_{mid} [rad])$
+- $dY [mm] = distanceCenter [mm] \cdot cos(orientation_{mid} [rad])$
+- $x_{new} [mm] = x_{old} [mm] + dX [mm]$
+- $y_{new} [mm] = y_{old} [mm] + dY [mm]$
+
+Fixed-point implementation:
+
+For better precision and to avoid unnecessary floating-point conversions, the implementation internally uses milliradians (mrad).
+
+Orientation:
 
 - $alpha [mrad] = \frac{1000 \cdot (encoderStepsRight [steps] - encoderStepsLeft [steps])}{encoderStepsPerMM [\frac{steps}{mm}] \cdot wheelBase [mm]}$
-- $orientation' [mrad] = orientation [mrad] + alpha [mrad]$
-- $dX [mm] = -distanceCenter [mm] \cdot sin(\frac{orientation' [mrad]}{1000})$
-- $dY [mm] = distanceCenter [mm] \cdot cos(\frac{orientation' [mrad]}{1000})$
+- $orientation_{new} [mrad] = orientation_{old} [mrad] + alpha [mrad]$
+- $orientation_{mid} [mrad] = orientation_{old} [mrad] + \frac{alpha [mrad]}{2}$
+
+Position:
+
+- $dX [steps] = -stepsCenter [steps] \cdot sin(\frac{orientation_{mid} [mrad]}{1000})$
+- $dY [steps] = stepsCenter [steps] \cdot cos(\frac{orientation_{mid} [mrad]}{1000})$
+
+To preserve sub-step precision, the calculated position increments are accumulated in units of $\frac{1}{1000}$ encoder steps before conversion to millimetres:
+
+- $dX_{1000} = 1000 \cdot dX [steps]$
+- $dY_{1000} = 1000 \cdot dY [steps]$
+
+The accumulated values are continuously converted to millimetres using the encoder resolution:
+
+- $x [mm] = \frac{\sum dX_{1000}}{encoderStepsPerMM \cdot 1000}$
+- $y [mm] = \frac{\sum dY_{1000}}{encoderStepsPerMM \cdot 1000}$
+
+This preserves fractional encoder-step contributions and significantly reduces long-term position drift caused by rounding.
 
 ##### Speedometer
 

lib/APPSensorFusion/src/ReadyState.cpp

@@ -72,12 +72,12 @@ LOG_TAG("RState");
 
 void ReadyState::entry()
 {
-
-    /* Clear the Odometry Position in favor of reproducibility. */
-    Odometry::getInstance().clearPosition();
-    Odometry::getInstance().setOrientation(1570);
     const int32_t SENSOR_VALUE_OUT_PERIOD = 1000; /* ms */
 
+    /* Clear the odometry position and orientation in favor of reproducibility. */
+    Odometry::getInstance().clearPosition();
+    Odometry::getInstance().clearOrientation();
+
     /* The line sensor value shall be output on console cyclic. */
     m_timer.start(SENSOR_VALUE_OUT_PERIOD);
 }

lib/Service/src/DifferentialDrive.cpp

@@ -248,15 +248,14 @@ void DifferentialDrive::calculateLinearSpeedLeftRight(int16_t linearSpeedCenter,
 {
     static const int32_t ANGULAR_TO_LINEAR_SPEED_FACTOR = 2000000;                                 /* 2 * 1000 mrad */
     int32_t              linearSpeedCenter32            = static_cast<int32_t>(linearSpeedCenter); /* [steps/s] */
-    int32_t              halfLinearSpeedCenter32        = linearSpeedCenter32 / 2;                 /* [steps/s] */
     int32_t              angularSpeed32                 = static_cast<int32_t>(angularSpeed);      /* [mrad/s] */
     int32_t              wheelBase32                    = static_cast<int32_t>(RobotConstants::WHEEL_BASE); /* [mm] */
     int32_t              encoderStepsPerM32 = static_cast<int32_t>(RobotConstants::ENCODER_STEPS_PER_M); /* [steps/m] */
     int32_t              linearSpeedTurnInPlace32 = (angularSpeed32 * wheelBase32 * encoderStepsPerM32) /
                                        static_cast<int32_t>(ANGULAR_TO_LINEAR_SPEED_FACTOR); /* [steps/s] */
 
-    linearSpeedLeft  = halfLinearSpeedCenter32 - linearSpeedTurnInPlace32; /* [steps/s] */
-    linearSpeedRight = halfLinearSpeedCenter32 + linearSpeedTurnInPlace32; /* [steps/s] */
+    linearSpeedLeft  = linearSpeedCenter32 - linearSpeedTurnInPlace32; /* [steps/s] */
+    linearSpeedRight = linearSpeedCenter32 + linearSpeedTurnInPlace32; /* [steps/s] */
 }
 
 void DifferentialDrive::calculateLinearAndAngularSpeedCenter(int16_t linearSpeedLeft, int16_t linearSpeedRight,

lib/Service/src/Odometry.cpp

@@ -84,20 +84,35 @@ void Odometry::process()
         if ((STEPS_THRESHOLD <= absStepsLeft) || (STEPS_THRESHOLD <= absStepsRight))
         {
             int16_t stepsCenter = static_cast<int16_t>((relStepsLeft + relStepsRight) / 2); /* [steps] */
-            int16_t dXSteps     = 0;                                                        /* [steps] */
-            int16_t dYSteps     = 0;                                                        /* [steps] */
+            int32_t dXSteps1000 = 0;                                                        /* [1/1000 steps] */
+            int32_t dYSteps1000 = 0;                                                        /* [1/1000 steps] */
             int32_t deltaPosX   = 0;                                                        /* [mm] */
             int32_t deltaPosY   = 0;                                                        /* [mm] */
 
+            /* Save orientation before update. */
+            int32_t oldOrientation = m_orientation;
+
+            /* Calculate orientation delta only. */
+            int32_t deltaOrientation = calculateOrientation(0, relStepsLeft, relStepsRight);
+
+            /* Use the orientation in the middle of the movement.
+             * This avoids the systematic position error caused by
+             * projecting the whole movement using the final heading.
+             */
+            int32_t orientationMid = oldOrientation + (deltaOrientation / 2);
+
             /* Calculate mileage in steps to avoid loosing precision by division. */
             m_mileage = calculateMileage(m_mileage, stepsCenter);
 
-            m_orientation = calculateOrientation(m_orientation, relStepsLeft, relStepsRight);
+            /* Calculate delta position in 1/1000 steps to avoid loosing precision by divison. */
+            calculateDeltaPos(stepsCenter, orientationMid, dXSteps1000, dYSteps1000);
 
-            /* Calculate delta position in steps to avoid loosing precision by divison. */
-            calculateDeltaPos(stepsCenter, m_orientation, dXSteps, dYSteps);
-            m_countingXSteps += dXSteps * 1000; /* Multiply with 1000 for higher precision. */
-            m_countingYSteps += dYSteps * 1000; /* Multiply with 1000 for higher precision. */
+            m_countingXSteps += dXSteps1000;
+            m_countingYSteps += dYSteps1000;
+
+            /* Update orientation afterwards. */
+            m_orientation = oldOrientation + deltaOrientation;
+            m_orientation %= FP_2PI();
 
             /* For large areas, its important to have the position in mm and not in steps.
              * Therefore the position in mm is continously calculated from the counted steps
@@ -156,6 +171,11 @@ void Odometry::clearPosition()
     m_countingYSteps          = 0;
 }
 
+void Odometry::clearOrientation()
+{
+    m_orientation = ORIENTATION_INITIAL;
+}
+
 void Odometry::clearMileage()
 {
     m_mileage = 0;
@@ -171,12 +191,13 @@ void Odometry::clearMileage()
 
 bool Odometry::detectStandStill(uint16_t absStepsLeft, uint16_t absStepsRight)
 {
-    bool isStandStill = false;
+    const uint16_t STANDSTILL_DETECTION_THRESHOLD = 1U; /* [steps] */
+    bool           isStandStill                   = false;
 
     /* No encoder (left/right) change detected? */
-    if (absStepsLeft == m_lastAbsRelEncStepsLeft)
+    if (abs(absStepsLeft - m_lastAbsRelEncStepsLeft) <= STANDSTILL_DETECTION_THRESHOLD)
     {
-        if (absStepsRight == m_lastAbsRelEncStepsRight)
+        if (abs(absStepsRight - m_lastAbsRelEncStepsRight) <= STANDSTILL_DETECTION_THRESHOLD)
         {
             isStandStill = true;
         }
@@ -235,12 +256,13 @@ int32_t Odometry::calculateOrientation(int32_t orientation, int16_t stepsLeft, i
     return orientation;
 }
 
-void Odometry::calculateDeltaPos(int16_t stepsCenter, int32_t orientation, int16_t& dXSteps, int16_t& dYSteps) const
+void Odometry::calculateDeltaPos(int16_t stepsCenter, int32_t orientation, int32_t& dXSteps1000,
+                                 int32_t& dYSteps1000) const
 {
-    float fDistCenter  = static_cast<float>(stepsCenter);           /* [steps] */
-    float fOrientation = static_cast<float>(orientation) / 1000.0F; /* [rad] */
-    float fDeltaPosX   = fDistCenter * cosf(fOrientation);          /* [steps] */
-    float fDeltaPosY   = fDistCenter * sinf(fOrientation);          /* [steps] */
+    float fDistCenter  = static_cast<float>(stepsCenter);            /* [steps] */
+    float fOrientation = static_cast<float>(orientation) / 1000.0F;  /* [rad] */
+    float fDeltaPosX   = fDistCenter * cosf(fOrientation) * 1000.0F; /* [1/1000 steps] */
+    float fDeltaPosY   = fDistCenter * sinf(fOrientation) * 1000.0F; /* [1/1000 steps] */
 
     /* Round because the cast will just cut the fractional part. */
     if (0.0F <= fDeltaPosX)
@@ -262,8 +284,8 @@ void Odometry::calculateDeltaPos(int16_t stepsCenter, int32_t orientation, int16
         fDeltaPosY -= 0.5F;
     }
 
-    dXSteps = static_cast<int16_t>(fDeltaPosX); /* [steps] */
-    dYSteps = static_cast<int16_t>(fDeltaPosY); /* [steps] */
+    dXSteps1000 = static_cast<int32_t>(fDeltaPosX); /* [1/1000 steps]*/
+    dYSteps1000 = static_cast<int32_t>(fDeltaPosY); /* [1/1000 steps]*/
 }
 
 /******************************************************************************

lib/Service/src/Odometry.h

@@ -154,6 +154,11 @@ class Odometry
      */
     void clearPosition();
 
+    /**
+     * Clear the orientation by setting it to 90°.
+     */
+    void clearOrientation();
+
     /**
      * Clear mileage by setting it to 0 mm.
      */
@@ -187,6 +192,12 @@ class Odometry
      */
     static const uint32_t STANDSTILL_DETECTION_PERIOD = 10;
 
+    /**
+     * Initial orientation in mrad.
+     * 90° - heading to north
+     */
+    static const int32_t ORIENTATION_INITIAL = FP_PI() / 2;
+
     /**
      * Last number of relative encoder steps left. Its used to avoid permanent
      * clearing of the relative encoders.
@@ -234,7 +245,7 @@ class Odometry
         m_lastAbsRelEncStepsRight(0),
         m_mileage(0),
         m_relEncoders(Board::getInstance().getEncoders()),
-        m_orientation(FP_PI() / 2), /* 90° - heading to north */
+        m_orientation(ORIENTATION_INITIAL),
         m_posX(0),
         m_posY(0),
         m_countingXSteps(0),
@@ -305,10 +316,10 @@ class Odometry
      *
      * @param[in]   stepsCenter Number of steps center
      * @param[in]   orientation Orientation in mrad
-     * @param[out]  dXSteps     Delta x-position on x-axis in steps
-     * @param[out]  dYSteps     Delta y-position on y-axis in steps
+     * @param[out]  dXSteps1000 Delta x-position on x-axis in 1/1000 steps
+     * @param[out]  dYSteps1000 Delta y-position on y-axis in 1/1000 steps
      */
-    void calculateDeltaPos(int16_t stepsCenter, int32_t orientation, int16_t& dXSteps, int16_t& dYSteps) const;
+    void calculateDeltaPos(int16_t stepsCenter, int32_t orientation, int32_t& dXSteps1000, int32_t& dYSteps1000) const;
 };
 
 /******************************************************************************