In odometry, drift is the growth of pose error over time due to integrating imperfect incremental motion. A useful decomposition is: \( \text{increment error} = \text{systematic (bias-like)} + \text{random (noise-like)} \). Systematic components (e.g., wrong wheel radius, wheelbase miscalibration, gyro bias) create repeatable distortions that typically accumulate linearly or quadratically with time/distance. Random components (quantization, thermal noise, contact micro-slip) produce diffusive growth (random walk).

We model the true incremental body-frame motion at step \( k \) as \( \mathbf{u}_k = [\Delta s_k,\; \Delta\theta_k]^\top \), and the estimated increment as \( \hat{\mathbf{u}}_k = \mathbf{u}_k + \delta\mathbf{u}_k \), where \( \delta\mathbf{u}_k \) contains both deterministic bias and random noise terms.

flowchart TD
  A["Encoder + IMU raw measurements"] --> B["Calibration parameters (r, b, scale, alignment)"]
  B --> C["Increment estimate (Delta s, Delta theta)"]
  A --> C
  C --> D["Discrete integration to pose (x, y, theta)"]
  D --> E["Pose drift grows with time/distance"]
  E --> F["Systematic sources: \nbias-like (repeatable)"]
  E --> G["Random sources: \nnoise-like (diffusive)"]
        

Throughout, we distinguish: \( \text{Bias} \) (nonzero mean, accumulates predictably) versus \( \text{Noise} \) (zero mean, accumulates in variance).

For a differential-drive base (from Lesson 1), the per-step arc-length increments (right/left wheel) are: \( \Delta s_R = r_R \Delta\phi_R \), \( \Delta s_L = r_L \Delta\phi_L \). The body-frame increment is \( \Delta s = (\Delta s_R + \Delta s_L)/2 \), \( \Delta\theta = (\Delta s_R - \Delta s_L)/b \), where \( b \) is wheel separation (track width).

2.1 Radius scale error produces linear distance drift

Suppose the estimator uses \( \hat{r} = r(1+\varepsilon_r) \) while the true wheel radius is \( r \). Then (for any wheel) the estimated distance increment is \( \widehat{\Delta s} = \hat{r}\Delta\phi = (1+\varepsilon_r)\Delta s \). Over a traveled distance \( S = \sum_k \Delta s_k \):

\[ \widehat{S} - S = \sum_{k} \left((1+\varepsilon_r)\Delta s_k - \Delta s_k\right) = \varepsilon_r \sum_k \Delta s_k = \varepsilon_r S. \]

Conclusion: distance drift from a constant scale error grows linearly with distance.

2.2 Wheel radius mismatch produces systematic curvature (even when commanded straight)

Let the true wheel radii be \( r_R = r(1+\varepsilon_R) \), \( r_L = r(1+\varepsilon_L) \), but the estimator assumes equal radii. For “straight” encoder angle increments where \( \Delta\phi_R = \Delta\phi_L = \Delta\phi \), the true heading increment is:

\[ \Delta\theta = \frac{r_R\Delta\phi - r_L\Delta\phi}{b} = \frac{r(\varepsilon_R-\varepsilon_L)\Delta\phi}{b}. \]

If \( \varepsilon_R \neq \varepsilon_L \), then \( \Delta\theta \neq 0 \): the robot follows an arc with nonzero curvature. Over many steps, this creates a repeatable sideways drift.

2.3 Wheelbase bias creates heading drift proportional to total rotation

If the estimator uses \( \hat{b}=b(1+\varepsilon_b) \), then \( \widehat{\Delta\theta} = (\Delta s_R-\Delta s_L)/\hat{b} = \Delta\theta/(1+\varepsilon_b) \). For small \( |\varepsilon_b| \), first-order expansion gives:

\[ \widehat{\Delta\theta} = \Delta\theta(1+\varepsilon_b)^{-1} \approx \Delta\theta(1-\varepsilon_b), \quad |\varepsilon_b| \ll 1. \]

Hence the per-step heading error is approximately \( \delta(\Delta\theta) \approx -\varepsilon_b \Delta\theta \), and the accumulated heading error scales with the total executed rotation.

2.4 Contact effects: slip, skid, and rolling constraint violations

Even with perfect calibration, wheel odometry assumes pure rolling without lateral slip. On low-friction surfaces or during aggressive maneuvers, the effective wheel-ground relationship becomes state dependent. A simple engineering model is: \( \Delta s_{R,\text{eff}} = \Delta s_R + w_{R,k} \), \( \Delta s_{L,\text{eff}} = \Delta s_L + w_{L,k} \), where \( w_{(\cdot),k} \) can be biased (persistent slip) and/or noisy (intermittent micro-slip).

Let the encoder have \( N \) ticks per revolution. The wheel angle is observed in discrete steps: \( \Delta\phi = (2\pi/N)\,n \), with integer tick count \( n \). A standard quantization model is: \( \Delta\phi_{\text{meas}} = \Delta\phi_{\text{true}} + q \), where \( q \sim \mathcal{U}(-\Delta/2,\Delta/2) \) and \( \Delta = 2\pi/N \).

3.1 Quantization variance (derivation)

For \( q \sim \mathcal{U}(-\Delta/2,\Delta/2) \), we have \( \mathbb{E}[q]=0 \) and

\[ \operatorname{Var}(q) = \mathbb{E}[q^2] = \int_{-\Delta/2}^{\Delta/2} \frac{q^2}{\Delta}\,dq = \frac{1}{\Delta}\left[\frac{q^3}{3}\right]_{-\Delta/2}^{\Delta/2} = \frac{\Delta^2}{12}. \]

Therefore, wheel distance quantization noise has variance \( \operatorname{Var}(r q) = r^2\Delta^2/12 \).

3.2 Why “zero-mean noise” still creates drift

Even if \( \mathbb{E}[\delta\mathbf{u}_k]=\mathbf{0} \), the integrated pose error behaves like a random walk in position and heading. For a scalar random walk \( e_{k+1} = e_k + \eta_k \) with i.i.d. \( \eta_k \), we have:

\[ \mathbb{E}[e_k] = 0,\quad \operatorname{Var}(e_k) = k\,\operatorname{Var}(\eta_0), \quad k \ge 1. \]

The standard deviation thus grows as \( \sqrt{k} \) (diffusion), which is perceived as drift in practice.

For ground robots, the gyroscope is typically the dominant IMU contributor to planar dead-reckoning drift because heading errors rotate the entire velocity vector. A standard gyro model is: \( \omega_{\text{meas}}(t) = \omega(t) + b_g + n_g(t) \), where \( b_g \) is (approximately) constant bias over short periods and \( n_g(t) \) is zero-mean noise.

4.1 Proof: constant gyro bias yields linear heading error

The estimated heading is \( \hat{\theta}(t)=\theta(0)+\int_0^t \omega_{\text{meas}}(\tau)\,d\tau \). The true heading is \( \theta(t)=\theta(0)+\int_0^t \omega(\tau)\,d\tau \). Subtracting gives:

\[ \tilde{\theta}(t) \equiv \hat{\theta}(t)-\theta(t) = \int_0^t \left(b_g + n_g(\tau)\right)\,d\tau = b_g t + \int_0^t n_g(\tau)\,d\tau. \]

Taking expectation and using \( \mathbb{E}[n_g(\tau)]=0 \):

\[ \mathbb{E}[\tilde{\theta}(t)] = b_g t. \]

Conclusion: a constant gyro bias produces a heading drift that grows linearly with time.

4.2 Proof: heading bias produces quadratic cross-track drift (small-angle regime)

Consider approximately straight motion with constant forward speed \( v \). The estimated velocity direction is rotated by heading error \( \tilde{\theta}(t) \). The lateral (cross-track) velocity error is approximately \( v\sin(\tilde{\theta}) \). For small angles, \( \sin(\tilde{\theta}) \approx \tilde{\theta} \), so:

\[ \dot{\tilde{y}}(t) \approx v\,\tilde{\theta}(t) \approx v\,b_g t. \]

Integrating with \( \tilde{y}(0)=0 \):

\[ \tilde{y}(t) \approx \int_0^t v\,b_g \tau\,d\tau = \frac{1}{2}v b_g t^2. \]

Conclusion: even a small constant gyro bias can cause quadratic position drift over time.

4.3 Accelerometer bias and why double integration is dangerous

Although many ground robots avoid full accelerometer-to-position double integration, the canonical drift law is important: with \( a_{\text{meas}}(t)=a(t)+b_a+n_a(t) \), the velocity error from constant bias is \( \tilde{v}(t)=b_a t \), and position error is \( \tilde{x}(t)=\tfrac{1}{2}b_a t^2 \). With white noise, the position variance grows rapidly (super-diffusive) due to repeated integration.

Let the true discrete-time kinematic update (midpoint integration) be:

\[ \begin{aligned} x_{k+1} &= x_k + \Delta s_k \cos\!\left(\theta_k + \tfrac{1}{2}\Delta\theta_k\right),\\ y_{k+1} &= y_k + \Delta s_k \sin\!\left(\theta_k + \tfrac{1}{2}\Delta\theta_k\right),\\ \theta_{k+1} &= \theta_k + \Delta\theta_k. \end{aligned} \]

The estimator uses \( \hat{\Delta s}_k = \Delta s_k + \delta s_k \) and \( \hat{\Delta\theta}_k = \Delta\theta_k + \delta\theta_k \). Define the pose error \( \delta\mathbf{x}_k = [\delta x_k,\delta y_k,\delta\theta_k^{(pose)}]^\top \), where \( \delta\theta_k^{(pose)} = \hat{\theta}_k-\theta_k \).

5.1 Linearization (engineering recursion)

Using first-order Taylor expansion about the true trajectory and neglecting second-order terms, one obtains the approximate recursion:

\[ \begin{aligned} \delta x_{k+1} &\approx \delta x_k - \Delta s_k \sin\!\left(\theta_k + \tfrac{1}{2}\Delta\theta_k\right)\left(\delta\theta_k^{(pose)} + \tfrac{1}{2}\delta\theta_k\right) + \cos\!\left(\theta_k + \tfrac{1}{2}\Delta\theta_k\right)\delta s_k,\\ \delta y_{k+1} &\approx \delta y_k + \Delta s_k \cos\!\left(\theta_k + \tfrac{1}{2}\Delta\theta_k\right)\left(\delta\theta_k^{(pose)} + \tfrac{1}{2}\delta\theta_k\right) + \sin\!\left(\theta_k + \tfrac{1}{2}\Delta\theta_k\right)\delta s_k,\\ \delta\theta_{k+1}^{(pose)} &\approx \delta\theta_k^{(pose)} + \delta\theta_k. \end{aligned} \]

This recursion explains the key mechanism: heading error feeds into position error through the \( \Delta s_k(\cdot)\delta\theta \) coupling, which is why gyro bias is often catastrophic for long runs.

flowchart TD
  A["Delta s error (delta_s)"] --> B["Position error grows ~ sum(delta_s)"]
  C["Delta theta error (delta_theta)"] --> D["Heading error accumulates ~ sum(delta_theta)"]
  D --> E["Heading rotates velocity direction"]
  E --> F["Position error grows ~ sum(Delta s * heading_error)"]
        

5.2 A practical bias-growth corollary

If the heading increment error has a constant bias component \( \delta\theta_k = \beta \), then:

\[ \delta\theta_k^{(pose)} \approx k\beta \quad \text{and thus typically} \quad \|\,[\delta x_k,\delta y_k]^\top\|\ \\ \text{grows at least on the order of}\ k^2 \ \text{for sustained motion}. \]

This is the discrete-time analogue of the quadratic cross-track drift proved earlier.

The following reference implementations simulate a differential-drive robot with: (i) wheel radius mismatch, (ii) wheelbase miscalibration, (iii) encoder quantization/noise, and (iv) gyro bias/noise. Compare “wheel-only” versus “gyro-heading + wheel-distance” dead reckoning to see how heading bias drives large drift.

Robotics-oriented libraries (where applicable): Python: \( \) NumPy, Matplotlib; ROS 2: rclpy, nav_msgs/Odometry, TF helpers. C++: Eigen; ROS 2: rclcpp, nav_msgs::msg::Odometry, tf2. Java: ROSJava (where used), plus standard math utilities. MATLAB/Simulink: Robotics System Toolbox; Simulink for bias-integration demonstration. Wolfram Mathematica: built-in numerical integration and plotting.

Note: in the webpage view, angle brackets in C++ headers are HTML-escaped; the downloadable source files contain literal characters.

6.1 Python

File: Chapter5_Lesson3.py

# Chapter5_Lesson3.py
# Autonomous Mobile Robots (Control Engineering) — Chapter 5, Lesson 3
# Topic: Drift Sources and Bias Accumulation
#
# Dependencies: numpy, matplotlib

import numpy as np
import matplotlib.pyplot as plt

def wrap_pi(a):
    return (a + np.pi) % (2*np.pi) - np.pi

def simulate(
    T=60.0, dt=0.01,
    r_true=0.05, b_true=0.30, ticks_per_rev=2048,
    r_hat=0.05*1.005, b_hat=0.30*0.995,
    eps_r_L=+0.002, eps_r_R=-0.002,
    encoder_tick_noise_std=0.2,
    gyro_bias=0.005,
    gyro_noise_std=0.002,
    seed=7
):
    rng = np.random.default_rng(seed)
    N = int(np.floor(T/dt))
    t = np.arange(N)*dt

    v_cmd = 0.6*np.ones(N)
    w_cmd = 0.10*np.sin(2*np.pi*t/20.0)

    rL_true = r_true*(1.0 + eps_r_L)
    rR_true = r_true*(1.0 + eps_r_R)
    rad_per_tick = 2*np.pi / ticks_per_rev

    x = np.zeros(N); y = np.zeros(N); th = np.zeros(N)
    xw = np.zeros(N); yw = np.zeros(N); thw = np.zeros(N)
    xg = np.zeros(N); yg = np.zeros(N); thg = np.zeros(N)
    th_gyro = 0.0

    for k in range(1, N):
        v = v_cmd[k-1]; w = w_cmd[k-1]

        wR = (v + 0.5*b_true*w)/rR_true
        wL = (v - 0.5*b_true*w)/rL_true

        dphiR_true = wR*dt
        dphiL_true = wL*dt

        ticksR = dphiR_true / rad_per_tick
        ticksL = dphiL_true / rad_per_tick
        ticksR_meas = np.round(ticksR) + rng.normal(0.0, encoder_tick_noise_std)
        ticksL_meas = np.round(ticksL) + rng.normal(0.0, encoder_tick_noise_std)

        dphiR_meas = ticksR_meas * rad_per_tick
        dphiL_meas = ticksL_meas * rad_per_tick

        dSR_true = rR_true*dphiR_true
        dSL_true = rL_true*dphiL_true
        dS_true  = 0.5*(dSR_true + dSL_true)
        dTh_true = (dSR_true - dSL_true)/b_true

        th[k] = wrap_pi(th[k-1] + dTh_true)
        x[k]  = x[k-1] + dS_true*np.cos(th[k-1] + 0.5*dTh_true)
        y[k]  = y[k-1] + dS_true*np.sin(th[k-1] + 0.5*dTh_true)

        dSR_hat = r_hat*dphiR_meas
        dSL_hat = r_hat*dphiL_meas
        dS_hat  = 0.5*(dSR_hat + dSL_hat)
        dTh_hat = (dSR_hat - dSL_hat)/b_hat

        thw[k] = wrap_pi(thw[k-1] + dTh_hat)
        xw[k]  = xw[k-1] + dS_hat*np.cos(thw[k-1] + 0.5*dTh_hat)
        yw[k]  = yw[k-1] + dS_hat*np.sin(thw[k-1] + 0.5*dTh_hat)

        w_meas = w + gyro_bias + rng.normal(0.0, gyro_noise_std)
        th_gyro = wrap_pi(th_gyro + w_meas*dt)

        thg[k] = th_gyro
        xg[k]  = xg[k-1] + dS_hat*np.cos(thg[k-1])
        yg[k]  = yg[k-1] + dS_hat*np.sin(thg[k-1])

    return t, (x,y,th), (xw,yw,thw), (xg,yg,thg)

def main():
    t, truth, wheel, gyro = simulate()
    xt, yt, tht = truth
    xw, yw, thw = wheel
    xg, yg, thg = gyro

    e_w = np.sqrt((xw-xt)**2 + (yw-yt)**2)
    e_g = np.sqrt((xg-xt)**2 + (yg-yt)**2)

    print("Final position error (wheel-only) [m]:", float(e_w[-1]))
    print("Final position error (gyro-heading) [m]:", float(e_g[-1]))

    plt.figure()
    plt.plot(xt, yt, label="truth")
    plt.plot(xw, yw, label="wheel-only")
    plt.plot(xg, yg, label="gyro-heading + wheel-distance")
    plt.axis("equal"); plt.grid(True)
    plt.xlabel("x [m]"); plt.ylabel("y [m]")
    plt.title("Dead-reckoning drift under bias and noise")
    plt.legend()

    plt.figure()
    plt.plot(t, e_w, label="wheel-only position error")
    plt.plot(t, e_g, label="gyro-heading position error")
    plt.grid(True)
    plt.xlabel("time [s]"); plt.ylabel("||position error|| [m]")
    plt.title("Error accumulation over time")
    plt.legend()

    plt.show()

if __name__ == "__main__":
    main()
      

6.2 C++

File: Chapter5_Lesson3.cpp

// Chapter5_Lesson3.cpp
// Build:
//   g++ -O2 -std=c++17 Chapter5_Lesson3.cpp -o ch5_l3

#include <iostream>
#include <vector>
#include <cmath>
#include <random>

static double wrap_pi(double a) {
    const double pi = 3.14159265358979323846;
    a = std::fmod(a + pi, 2.0*pi);
    if (a < 0) a += 2.0*pi;
    return a - pi;
}

struct Result {
    double final_pos_err_wheel;
    double final_pos_err_gyro;
    double final_head_err_wheel_deg;
    double final_head_err_gyro_deg;
};

Result simulate(
    double T=60.0, double dt=0.01,
    double r_true=0.05, double b_true=0.30, int ticks_per_rev=2048,
    double r_hat=0.05*1.005, double b_hat=0.30*0.995,
    double eps_r_L=+0.002, double eps_r_R=-0.002,
    double encoder_tick_noise_std=0.2,
    double gyro_bias=0.005, double gyro_noise_std=0.002,
    unsigned seed=7
) {
    int N = (int)std::floor(T/dt);
    std::mt19937 gen(seed);
    std::normal_distribution<double> n01(0.0, 1.0);

    const double pi = 3.14159265358979323846;
    const double rad_per_tick = 2.0*pi / (double)ticks_per_rev;

    double rL_true = r_true*(1.0 + eps_r_L);
    double rR_true = r_true*(1.0 + eps_r_R);

    double x=0, y=0, th=0;
    double xw=0, yw=0, thw=0;
    double xg=0, yg=0, thg=0;
    double th_gyro=0;

    for (int k=1; k<N; ++k) {
        double t = (k-1)*dt;
        double v = 0.6;
        double w = 0.10*std::sin(2.0*pi*t/20.0);

        double wR = (v + 0.5*b_true*w)/rR_true;
        double wL = (v - 0.5*b_true*w)/rL_true;

        double dphiR_true = wR*dt;
        double dphiL_true = wL*dt;

        double ticksR = dphiR_true / rad_per_tick;
        double ticksL = dphiL_true / rad_per_tick;

        double ticksR_meas = std::round(ticksR) + encoder_tick_noise_std*n01(gen);
        double ticksL_meas = std::round(ticksL) + encoder_tick_noise_std*n01(gen);

        double dphiR_meas = ticksR_meas * rad_per_tick;
        double dphiL_meas = ticksL_meas * rad_per_tick;

        double dSR_true = rR_true*dphiR_true;
        double dSL_true = rL_true*dphiL_true;
        double dS_true  = 0.5*(dSR_true + dSL_true);
        double dTh_true = (dSR_true - dSL_true)/b_true;

        th = wrap_pi(th + dTh_true);
        x  = x + dS_true*std::cos(th - 0.5*dTh_true);
        y  = y + dS_true*std::sin(th - 0.5*dTh_true);

        double dSR_hat = r_hat*dphiR_meas;
        double dSL_hat = r_hat*dphiL_meas;
        double dS_hat  = 0.5*(dSR_hat + dSL_hat);
        double dTh_hat = (dSR_hat - dSL_hat)/b_hat;

        thw = wrap_pi(thw + dTh_hat);
        xw  = xw + dS_hat*std::cos(thw - 0.5*dTh_hat);
        yw  = yw + dS_hat*std::sin(thw - 0.5*dTh_hat);

        double w_meas = w + gyro_bias + gyro_noise_std*n01(gen);
        th_gyro = wrap_pi(th_gyro + w_meas*dt);
        thg = th_gyro;

        xg = xg + dS_hat*std::cos(thg);
        yg = yg + dS_hat*std::sin(thg);
    }

    double e_w = std::sqrt((xw-x)*(xw-x) + (yw-y)*(yw-y));
    double e_g = std::sqrt((xg-x)*(xg-x) + (yg-y)*(yg-y));
    double he_w = (180.0/pi)*wrap_pi(thw-th);
    double he_g = (180.0/pi)*wrap_pi(thg-th);

    return {e_w, e_g, he_w, he_g};
}

int main() {
    Result r = simulate();
    std::cout << "Final position error (wheel-only) [m]: " << r.final_pos_err_wheel << "\n";
    std::cout << "Final position error (gyro-heading) [m]: " << r.final_pos_err_gyro << "\n";
    std::cout << "Final heading error (wheel-only) [deg]: " << r.final_head_err_wheel_deg << "\n";
    std::cout << "Final heading error (gyro-heading) [deg]: " << r.final_head_err_gyro_deg << "\n";
    return 0;
}
      

6.3 Java

File: Chapter5_Lesson3.java

// Chapter5_Lesson3.java
// Compile: javac Chapter5_Lesson3.java
// Run:     java Chapter5_Lesson3

import java.util.Random;

public class Chapter5_Lesson3 {

    static double wrapPi(double a) {
        double pi = Math.PI;
        a = (a + pi) % (2.0*pi);
        if (a < 0) a += 2.0*pi;
        return a - pi;
    }

    static class Result {
        double posErrWheel, posErrGyro, headErrWheelDeg, headErrGyroDeg;
    }

    static Result simulate(
        double T, double dt,
        double rTrue, double bTrue, int ticksPerRev,
        double rHat, double bHat,
        double epsRL, double epsRR,
        double encTickNoiseStd,
        double gyroBias, double gyroNoiseStd,
        long seed
    ) {
        int N = (int)Math.floor(T/dt);
        Random rng = new Random(seed);

        double radPerTick = 2.0*Math.PI / (double)ticksPerRev;
        double rLTrue = rTrue*(1.0 + epsRL);
        double rRTrue = rTrue*(1.0 + epsRR);

        double x=0, y=0, th=0;
        double xw=0, yw=0, thw=0;
        double xg=0, yg=0, thg=0;
        double thGyro=0;

        for (int k=1; k<N; k++) {
            double t = (k-1)*dt;
            double v = 0.6;
            double w = 0.10*Math.sin(2.0*Math.PI*t/20.0);

            double wR = (v + 0.5*bTrue*w)/rRTrue;
            double wL = (v - 0.5*bTrue*w)/rLTrue;

            double dphiRTrue = wR*dt;
            double dphiLTrue = wL*dt;

            double ticksR = dphiRTrue / radPerTick;
            double ticksL = dphiLTrue / radPerTick;

            // Gaussian via Box-Muller
            double nR = Math.sqrt(-2.0*Math.log(Math.max(1e-12, rng.nextDouble()))) * Math.cos(2.0*Math.PI*rng.nextDouble());
            double nL = Math.sqrt(-2.0*Math.log(Math.max(1e-12, rng.nextDouble()))) * Math.cos(2.0*Math.PI*rng.nextDouble());

            double ticksRMeas = Math.rint(ticksR) + encTickNoiseStd*nR;
            double ticksLMeas = Math.rint(ticksL) + encTickNoiseStd*nL;

            double dphiRMeas = ticksRMeas * radPerTick;
            double dphiLMeas = ticksLMeas * radPerTick;

            double dSRTrue = rRTrue*dphiRTrue;
            double dSLTrue = rLTrue*dphiLTrue;
            double dSTrue  = 0.5*(dSRTrue + dSLTrue);
            double dThTrue = (dSRTrue - dSLTrue)/bTrue;

            th = wrapPi(th + dThTrue);
            x  = x + dSTrue*Math.cos(th - 0.5*dThTrue);
            y  = y + dSTrue*Math.sin(th - 0.5*dThTrue);

            double dSRHat = rHat*dphiRMeas;
            double dSLHat = rHat*dphiLMeas;
            double dSHat  = 0.5*(dSRHat + dSLHat);
            double dThHat = (dSRHat - dSLHat)/bHat;

            thw = wrapPi(thw + dThHat);
            xw  = xw + dSHat*Math.cos(thw - 0.5*dThHat);
            yw  = yw + dSHat*Math.sin(thw - 0.5*dThHat);

            double nG = Math.sqrt(-2.0*Math.log(Math.max(1e-12, rng.nextDouble()))) * Math.cos(2.0*Math.PI*rng.nextDouble());
            double wMeas = w + gyroBias + gyroNoiseStd*nG;
            thGyro = wrapPi(thGyro + wMeas*dt);

            thg = thGyro;
            xg = xg + dSHat*Math.cos(thg);
            yg = yg + dSHat*Math.sin(thg);
        }

        Result out = new Result();
        out.posErrWheel = Math.sqrt((xw-x)*(xw-x) + (yw-y)*(yw-y));
        out.posErrGyro  = Math.sqrt((xg-x)*(xg-x) + (yg-y)*(yg-y));
        out.headErrWheelDeg = (180.0/Math.PI)*wrapPi(thw-th);
        out.headErrGyroDeg  = (180.0/Math.PI)*wrapPi(thg-th);
        return out;
    }

    public static void main(String[] args) {
        Result r = simulate(
            60.0, 0.01,
            0.05, 0.30, 2048,
            0.05*1.005, 0.30*0.995,
            +0.002, -0.002,
            0.2,
            0.005, 0.002,
            7L
        );

        System.out.println("Final position error (wheel-only) [m]: " + r.posErrWheel);
        System.out.println("Final position error (gyro-heading) [m]: " + r.posErrGyro);
        System.out.println("Final heading error (wheel-only) [deg]: " + r.headErrWheelDeg);
        System.out.println("Final heading error (gyro-heading) [deg]: " + r.headErrGyroDeg);
    }
}
      

6.4 MATLAB / Simulink

File: Chapter5_Lesson3.m

% Chapter5_Lesson3.m
% Autonomous Mobile Robots — Chapter 5, Lesson 3
% Drift Sources and Bias Accumulation

clear; clc;

T  = 60.0;
dt = 0.01;
N  = floor(T/dt);
t  = (0:N-1)'*dt;

r_true = 0.05; b_true = 0.30; ticks_per_rev = 2048;
r_hat  = r_true*1.005; b_hat = b_true*0.995;

eps_r_L = +0.002; eps_r_R = -0.002;
rL_true = r_true*(1+eps_r_L);
rR_true = r_true*(1+eps_r_R);

rad_per_tick = 2*pi/ticks_per_rev;
encoder_tick_noise_std = 0.2;

gyro_bias = 0.005;
gyro_noise_std = 0.002;

rng(7);

v_cmd = 0.6*ones(N,1);
w_cmd = 0.10*sin(2*pi*t/20.0);

x=zeros(N,1); y=zeros(N,1); th=zeros(N,1);
xw=zeros(N,1); yw=zeros(N,1); thw=zeros(N,1);
xg=zeros(N,1); yg=zeros(N,1); thg=zeros(N,1);
th_gyro = 0;

wrapPi = @(a) mod(a+pi,2*pi)-pi;

for k=2:N
    v = v_cmd(k-1); w = w_cmd(k-1);

    wR = (v + 0.5*b_true*w)/rR_true;
    wL = (v - 0.5*b_true*w)/rL_true;

    dphiR_true = wR*dt;
    dphiL_true = wL*dt;

    ticksR = dphiR_true/rad_per_tick;
    ticksL = dphiL_true/rad_per_tick;

    ticksR_meas = round(ticksR) + encoder_tick_noise_std*randn();
    ticksL_meas = round(ticksL) + encoder_tick_noise_std*randn();

    dphiR_meas = ticksR_meas*rad_per_tick;
    dphiL_meas = ticksL_meas*rad_per_tick;

    dSR_true = rR_true*dphiR_true;
    dSL_true = rL_true*dphiL_true;
    dS_true  = 0.5*(dSR_true + dSL_true);
    dTh_true = (dSR_true - dSL_true)/b_true;

    th(k) = wrapPi(th(k-1) + dTh_true);
    x(k)  = x(k-1) + dS_true*cos(th(k-1) + 0.5*dTh_true);
    y(k)  = y(k-1) + dS_true*sin(th(k-1) + 0.5*dTh_true);

    dSR_hat = r_hat*dphiR_meas;
    dSL_hat = r_hat*dphiL_meas;
    dS_hat  = 0.5*(dSR_hat + dSL_hat);
    dTh_hat = (dSR_hat - dSL_hat)/b_hat;

    thw(k) = wrapPi(thw(k-1) + dTh_hat);
    xw(k)  = xw(k-1) + dS_hat*cos(thw(k-1) + 0.5*dTh_hat);
    yw(k)  = yw(k-1) + dS_hat*sin(thw(k-1) + 0.5*dTh_hat);

    w_meas = w + gyro_bias + gyro_noise_std*randn();
    th_gyro = wrapPi(th_gyro + w_meas*dt);

    thg(k) = th_gyro;
    xg(k)  = xg(k-1) + dS_hat*cos(thg(k-1));
    yg(k)  = yg(k-1) + dS_hat*sin(thg(k-1));
end

e_w = sqrt((xw-x).^2 + (yw-y).^2);
e_g = sqrt((xg-x).^2 + (yg-y).^2);

fprintf('Final position error (wheel-only) [m]: %.4f\n', e_w(end));
fprintf('Final position error (gyro-heading) [m]: %.4f\n', e_g(end));

figure; plot(x,y,'LineWidth',1.5); hold on;
plot(xw,yw,'LineWidth',1.2);
plot(xg,yg,'LineWidth',1.2);
axis equal; grid on;
xlabel('x [m]'); ylabel('y [m]');
title('Dead-reckoning drift under bias and noise');
legend('truth','wheel-only','gyro-heading + wheel-distance');

% Optional Simulink build is included in the downloadable .m file (disabled by default).
      

6.5 Wolfram Mathematica

File: Chapter5_Lesson3.nb

(* Chapter5_Lesson3.nb
   Paste into a Mathematica notebook cell, evaluate, then save as .nb. *)

Notebook[{
  Cell["Chapter 5 — Lesson 3: Drift Sources and Bias Accumulation", "Title"],
  Cell["Simulation: differential-drive dead-reckoning with bias/noise", "Section"],

  Cell[BoxData @ ToBoxes @ HoldForm[
    ClearAll[wrapPi, simulate];
    wrapPi[a_] := Mod[a + Pi, 2 Pi] - Pi;

    simulate[T_: 60., dt_: 0.01,
      rTrue_: 0.05, bTrue_: 0.30, ticksPerRev_: 2048,
      rHat_: 0.05*1.005, bHat_: 0.30*0.995,
      epsRL_: 0.002, epsRR_: -0.002,
      encTickNoiseStd_: 0.2,
      gyroBias_: 0.005, gyroNoiseStd_: 0.002, seed_: 7] := Module[
      {N, t, vCmd, wCmd, rLTrue, rRTrue, radPerTick,
       x, y, th, xw, yw, thw, xg, yg, thg, thGyro, k,
       v, w, wR, wL, dphiRTrue, dphiLTrue, ticksR, ticksL,
       ticksRMeas, ticksLMeas, dphiRMeas, dphiLMeas,
       dSRTrue, dSLTrue, dSTrue, dThTrue, dSRHat, dSLHat, dSHat, dThHat,
       wMeas},

      SeedRandom[seed];
      N = Floor[T/dt];
      t = Range[0, N-1]*dt;

      vCmd = ConstantArray[0.6, N];
      wCmd = 0.10*Sin[2 Pi t/20.0];

      rLTrue = rTrue*(1 + epsRL);
      rRTrue = rTrue*(1 + epsRR);
      radPerTick = 2 Pi/ticksPerRev;

      x = ConstantArray[0., N]; y = ConstantArray[0., N]; th = ConstantArray[0., N];
      xw = ConstantArray[0., N]; yw = ConstantArray[0., N]; thw = ConstantArray[0., N];
      xg = ConstantArray[0., N]; yg = ConstantArray[0., N]; thg = ConstantArray[0., N];
      thGyro = 0.;

      For[k = 2, k <= N, k++,
        v = vCmd[[k-1]]; w = wCmd[[k-1]];

        wR = (v + 0.5*bTrue*w)/rRTrue;
        wL = (v - 0.5*bTrue*w)/rLTrue;

        dphiRTrue = wR*dt; dphiLTrue = wL*dt;

        ticksR = dphiRTrue/radPerTick;
        ticksL = dphiLTrue/radPerTick;

        ticksRMeas = Round[ticksR] + encTickNoiseStd*RandomVariate[NormalDistribution[0,1]];
        ticksLMeas = Round[ticksL] + encTickNoiseStd*RandomVariate[NormalDistribution[0,1]];

        dphiRMeas = ticksRMeas*radPerTick;
        dphiLMeas = ticksLMeas*radPerTick;

        dSRTrue = rRTrue*dphiRTrue;
        dSLTrue = rLTrue*dphiLTrue;
        dSTrue  = 0.5*(dSRTrue + dSLTrue);
        dThTrue = (dSRTrue - dSLTrue)/bTrue;

        th[[k]] = wrapPi[th[[k-1]] + dThTrue];
        x[[k]]  = x[[k-1]] + dSTrue*Cos[th[[k-1]] + 0.5*dThTrue];
        y[[k]]  = y[[k-1]] + dSTrue*Sin[th[[k-1]] + 0.5*dThTrue];

        dSRHat = rHat*dphiRMeas;
        dSLHat = rHat*dphiLMeas;
        dSHat  = 0.5*(dSRHat + dSLHat);
        dThHat = (dSRHat - dSLHat)/bHat;

        thw[[k]] = wrapPi[thw[[k-1]] + dThHat];
        xw[[k]]  = xw[[k-1]] + dSHat*Cos[thw[[k-1]] + 0.5*dThHat];
        yw[[k]]  = yw[[k-1]] + dSHat*Sin[thw[[k-1]] + 0.5*dThHat];

        wMeas = w + gyroBias + gyroNoiseStd*RandomVariate[NormalDistribution[0,1]];
        thGyro = wrapPi[thGyro + wMeas*dt];

        thg[[k]] = thGyro;
        xg[[k]]  = xg[[k-1]] + dSHat*Cos[thg[[k-1]]];
        yg[[k]]  = yg[[k-1]] + dSHat*Sin[thg[[k-1]]];
      ];

      <|
        "t" -> t,
        "truth" -> Transpose[{x,y,th}],
        "wheel" -> Transpose[{xw,yw,thw}],
        "gyro"  -> Transpose[{xg,yg,thg}]
      |>
    ];

    data = simulate[];
    truth = data["truth"]; wheel = data["wheel"]; gyro = data["gyro"];

    posErrWheel = Norm /@ (wheel[[All, {1,2}]] - truth[[All, {1,2}]]);
    posErrGyro  = Norm /@ (gyro[[All, {1,2}]] - truth[[All, {1,2}]]);
  ], "Input"]
}]
      

Problem 1 (Linear drift from wheel-radius scale error): Assume the estimator uses \( \hat{r} = r(1+\varepsilon_r) \) for both wheels, while the true radius is \( r \). Show that the distance error after traveling true distance \( S \) is \( \widehat{S}-S=\varepsilon_r S \).

Solution: For each increment, \( \widehat{\Delta s}_k = \hat{r}\Delta\phi_k=(1+\varepsilon_r)\Delta s_k \). Summing:

\[ \widehat{S}-S = \sum_k \left(\widehat{\Delta s}_k - \Delta s_k\right) = \sum_k \varepsilon_r \Delta s_k = \varepsilon_r \sum_k \Delta s_k = \varepsilon_r S. \]

Problem 2 (Curvature from wheel-radius mismatch): For a differential drive, suppose \( r_R=r(1+\varepsilon_R) \), \( r_L=r(1+\varepsilon_L) \), and the wheels rotate equally: \( \Delta\phi_R=\Delta\phi_L=\Delta\phi \). Derive \( \Delta\theta \) and state when the robot is truly straight.

Solution:

\[ \Delta\theta = \frac{r_R\Delta\phi - r_L\Delta\phi}{b} = \frac{r(\varepsilon_R-\varepsilon_L)\Delta\phi}{b}. \]

The motion is straight iff \( \varepsilon_R=\varepsilon_L \) (or trivially \( \Delta\phi=0 \)).

Problem 3 (Quantization variance): Let quantization error be \( q \sim \mathcal{U}(-\Delta/2,\Delta/2) \). Prove \( \operatorname{Var}(q)=\Delta^2/12 \).

Solution: Since \( \mathbb{E}[q]=0 \),

\[ \operatorname{Var}(q) = \int_{-\Delta/2}^{\Delta/2}\frac{q^2}{\Delta}\,dq = \frac{1}{\Delta}\left[\frac{q^3}{3}\right]_{-\Delta/2}^{\Delta/2} = \frac{1}{\Delta}\left(\frac{\Delta^3}{24}+\frac{\Delta^3}{24}\right) = \frac{\Delta^2}{12}. \]

Problem 4 (Quadratic cross-track drift from gyro bias): Assume straight motion at constant speed \( v \), and a constant gyro bias \( b_g \) so that \( \tilde{\theta}(t)=b_g t \). Using \( \sin(\tilde{\theta})\approx \tilde{\theta} \) for small angles, derive \( \tilde{y}(t)\approx \tfrac{1}{2}v b_g t^2 \).

Solution: Lateral velocity error is \( \dot{\tilde{y}} \approx v\sin(\tilde{\theta}) \approx v\tilde{\theta}=v b_g t \). Integrate:

\[ \tilde{y}(t) \approx \int_0^t v b_g \tau\,d\tau = \frac{1}{2}v b_g t^2. \]

Problem 5 (Discrete-time heading bias accumulation): In the recursion \( \delta\theta_{k+1}^{(pose)} = \delta\theta_k^{(pose)} + \delta\theta_k \), assume \( \delta\theta_k=\beta \) (constant). Show \( \delta\theta_k^{(pose)} = k\beta \) for \( k \ge 0 \) given \( \delta\theta_0^{(pose)}=0 \).

Solution: By induction. Base: \( k=0 \Rightarrow \delta\theta_0^{(pose)}=0 \). Step: if \( \delta\theta_k^{(pose)}=k\beta \), then \( \delta\theta_{k+1}^{(pose)} = k\beta + \beta = (k+1)\beta \).

We identified the dominant drift sources in dead reckoning and separated systematic (bias-like) from random (noise-like) effects. We proved linear distance drift from scale errors, linear heading drift from gyro bias, and quadratic cross-track drift induced by heading bias. We also derived a first-order SE(2) error propagation recursion that explains why heading errors strongly couple into position drift. These results motivate the need for dedicated odometry filtering and calibration procedures (next lesson) and, later, probabilistic state estimation.