In Chapters 2–3 we modeled motion feasibility primarily through kinematics and constraints. In Chapter 4 we introduced applied dynamics: traction limits, slip, simple dynamic models, and stability constraints. We now connect these ideas to what a motion execution layer (and later, full navigation) experiences on hardware: the executed motion differs from the commanded motion because the mapping \( (\text{commands}) \mapsto (\text{body motion}) \) depends on physical parameters.

We collect the dominant parameters affecting “real navigation” for a wheeled ground robot as \(\mathbf{p}\):

\[ \mathbf{p} = \begin{bmatrix} m & I_z & r & b & \mu & c_v & c_\omega & h & w \end{bmatrix}^\top \]

where \(m\) is mass, \(I_z\) yaw inertia, \(r\) wheel radius, \(b\) track (left–right wheel separation), \(\mu\) friction coefficient, \(c_v\) and \(c_\omega\) lumped damping terms, \(h\) center-of-mass height, and \(w\) support polygon width.

flowchart TD
  A["Commanded motion: v_d(t), omega_d(t)"] --> B["Low-level control computes wheel torques"]
  B --> C["Robot + terrain dynamics"]
  C --> D["Executed motion: \nv(t), omega(t)"]
  D --> E["Pose integration: \nx(t), y(t), theta(t)"]
  C --> F["Parameter variations: \nm, Iz, r, b, mu, h, w"]
  F --> C
  D --> G["Tracking error + \nsafety constraint violations"]
        

The central engineering question of this lesson is: given nominal parameters \(\mathbf{p}_0\) and true parameters \(\mathbf{p} = \mathbf{p}_0 + \delta\mathbf{p}\), how large can the deviation in pose and velocities become over a finite horizon?

We use the same “simple dynamic model” spirit from Chapter 4, Lesson 3: we keep the planar kinematics exact, and we model longitudinal and yaw dynamics by first-order linear terms plus wheel-torque actuation. The state is \(\mathbf{x} = [x,\; y,\; \theta,\; v,\; \omega]^\top\).

\[ \begin{aligned} \dot x &= v\cos\theta,\qquad \dot y = v\sin\theta,\qquad \dot\theta = \omega \\ \dot v &= \frac{1}{m}F - c_v v,\qquad \dot\omega = \frac{1}{I_z}M_z - c_\omega \omega \end{aligned} \]

For a differential-drive base with left/right wheel torques \(\tau_L,\tau_R\), we use the standard torque-to-wrench mapping:

\[ F = \frac{\tau_R + \tau_L}{r},\qquad M_z = \frac{b}{2r}(\tau_R - \tau_L). \]

Traction saturation. On flat terrain, the total longitudinal force is bounded by Coulomb friction:

\[ |F| \le \mu m g. \]

In this lesson we treat slip onset as a hard saturation, which is sufficient to understand parameter effects and controller robustness.

Consider the longitudinal channel (the yaw channel is analogous). Let the low-level controller compute a generalized force command \(F\) to track a reference velocity \(v_d(t)\):

\[ e_v := v_d - v,\qquad F := m_0\big(k_p e_v + k_i \eta_v\big) + m_0 c_{v,0} v,\qquad \dot\eta_v = e_v, \]

where \(m_0, c_{v,0}\) are nominal parameters used inside the controller. Substituting into the true plant \(\dot v = \frac{1}{m}F - c_v v\) yields the error dynamics (ignoring saturation):

\[ \dot e_v = -\frac{m_0}{m}k_p e_v - \frac{m_0}{m}k_i \eta_v -\big(c_v - \frac{m_0}{m}c_{v,0}\big)v. \]

If we assume perfect feedforward cancellation \(c_v = c_{v,0}\) and constant reference, then the characteristic polynomial is:

\[ s^2 + \frac{m_0}{m}k_p\, s + \frac{m_0}{m}k_i = 0. \]

Stability proof (applied). For any \(k_p > 0\), \(k_i > 0\), and physical mass \(m > 0\), the polynomial is Hurwitz (second-order Routh–Hurwitz), so the longitudinal channel is exponentially stable. However, the transient response depends on \(\sqrt{m_0/m}\).

Let the closed-loop robot (including controller) be represented as \(\dot{\mathbf{x}} = \mathbf{f}(\mathbf{x}, t, \mathbf{p})\). We linearize the effect of parameter perturbations around a nominal trajectory \(\mathbf{x}_0(t)\) generated with \(\mathbf{p}_0\).

\[ \mathbf{x}(t) = \mathbf{x}_0(t) + \delta\mathbf{x}(t),\qquad \mathbf{p} = \mathbf{p}_0 + \delta\mathbf{p}. \]

Theorem (first-order parameter variation). If \(\mathbf{f}\) is continuously differentiable in \(\mathbf{x}\) and \(\mathbf{p}\), then:

\[ \delta\dot{\mathbf{x}}(t) = \mathbf{A}(t)\,\delta\mathbf{x}(t) + \mathbf{G}(t)\,\delta\mathbf{p} + \mathcal{O}\big(\lVert\delta\mathbf{x}\rVert^2 + \lVert\delta\mathbf{p}\rVert^2\big), \]

with Jacobians evaluated on the nominal trajectory: \(\mathbf{A}(t) := \frac{\partial \mathbf{f}}{\partial \mathbf{x}}(\mathbf{x}_0(t),t,\mathbf{p}_0)\) and \(\mathbf{G}(t) := \frac{\partial \mathbf{f}}{\partial \mathbf{p}}(\mathbf{x}_0(t),t,\mathbf{p}_0)\).

Proof. Apply a first-order Taylor expansion of \(\mathbf{f}\) around \((\mathbf{x}_0(t),\mathbf{p}_0)\):

\[ \mathbf{f}(\mathbf{x}_0 + \delta\mathbf{x}, t, \mathbf{p}_0 + \delta\mathbf{p}) = \mathbf{f}(\mathbf{x}_0,t,\mathbf{p}_0) + \mathbf{A}(t)\,\delta\mathbf{x} + \mathbf{G}(t)\,\delta\mathbf{p} + \mathcal{O}\big(\lVert\delta\mathbf{x}\rVert^2 + \lVert\delta\mathbf{p}\rVert^2\big). \]

Since \(\dot{\mathbf{x}}_0 = \mathbf{f}(\mathbf{x}_0,t,\mathbf{p}_0)\), subtract the nominal dynamics from the perturbed dynamics. □

Practical bound. For the first-order system \(\delta\dot{\mathbf{x}} = \mathbf{A}(t)\delta\mathbf{x} + \mathbf{G}(t)\delta\mathbf{p}\), if \(\lVert\mathbf{A}(t)\rVert \le a(t)\) and \(\lVert\mathbf{G}(t)\rVert \le g(t)\) for \(0 \le t \le T\), then:

\[ \lVert\delta\mathbf{x}(t)\rVert \le \exp\Big(\int_0^t a(\tau)\,d\tau\Big) \left( \lVert\delta\mathbf{x}(0)\rVert + \lVert\delta\mathbf{p}\rVert\int_0^t g(\tau)\,d\tau \right). \]

Closed-form first-order solution. Let \(\boldsymbol{\Phi}(t,\tau)\) be the state-transition matrix of \(\dot{\mathbf{z}}=\mathbf{A}(t)\mathbf{z}\). Then the first-order perturbation admits the variation-of-constants representation:

\[ \delta\mathbf{x}(t) = \boldsymbol{\Phi}(t,0)\,\delta\mathbf{x}(0) + \int_{0}^{t} \boldsymbol{\Phi}(t,\tau)\,\mathbf{G}(\tau)\,\delta\mathbf{p}\; d\tau. \]

Proof sketch. Differentiate the right-hand side and use \(\frac{\partial}{\partial t}\boldsymbol{\Phi}(t,\tau)=\mathbf{A}(t)\boldsymbol{\Phi}(t,\tau)\) and \(\boldsymbol{\Phi}(\tau,\tau)=\mathbf{I}\). This yields \(\delta\dot{\mathbf{x}}=\mathbf{A}(t)\delta\mathbf{x}+\mathbf{G}(t)\delta\mathbf{p}\). □

For differential drive, the mapping from wheel angular velocities \(\omega_L,\omega_R\) to body velocities is:

\[ v = \frac{r}{2}(\omega_R + \omega_L),\qquad \omega = \frac{r}{b}(\omega_R - \omega_L). \]

Suppose software uses estimated parameters \(\hat r = r(1+\varepsilon_r)\), \(\hat b = b(1+\varepsilon_b)\). Then:

\[ \hat v = (1+\varepsilon_r) v, \qquad \hat\omega = \frac{1+\varepsilon_r}{1+\varepsilon_b}\,\omega. \]

Using \(\frac{1+\varepsilon_r}{1+\varepsilon_b} \approx 1+\varepsilon_r-\varepsilon_b\) for \(|\varepsilon_r|,|\varepsilon_b| \ll 1\):

\[ \hat\omega - \omega \approx (\varepsilon_r - \varepsilon_b)\,\omega. \]

Proposition (heading drift). If heading is obtained by integrating \(\hat\omega\), then:

\[ \Delta\theta(T) := \hat\theta(T) - \theta(T) \approx (\varepsilon_r - \varepsilon_b)\int_0^T \omega(t)\,dt. \]

Proof. \(\Delta\theta(T) = \int_0^T (\hat\omega(t)-\omega(t))\,dt\) and substitute the approximation. □

Traction saturation implies:

\[ |\dot v + c_v v| = \left|\frac{F}{m}\right| \le \mu g. \]

If each wheel supports half the normal load, then \( |F_L| \le \mu \frac{mg}{2}\) and \( |F_R| \le \mu \frac{mg}{2}\), hence:

\[ |M_z| = \left|\frac{b}{2}(F_R - F_L)\right| \le \frac{b}{2}\left(|F_R| + |F_L|\right) \le \mu m g\,\frac{b}{2}. \]

Therefore:

\[ |\dot\omega + c_\omega \omega| \le \mu m g\,\frac{b}{2I_z}. \]

Motor/drive saturation (actuator-limited feasibility). Even with high friction, the drivetrain imposes a torque limit \(|\tau_L| \le \tau_{max}\), \(|\tau_R| \le \tau_{max}\). Since \(F = (\tau_R+\tau_L)/r\), we have:

\[ |F| \le \frac{|\tau_R|+|\tau_L|}{r} \le \frac{2\tau_{max}}{r}. \]

Combining with \(\dot v = \frac{1}{m}F - c_v v\) yields an actuator-limited acceleration bound:

\[ |\dot v + c_v v| \le \frac{2\tau_{max}}{mr}. \]

The effective bound is the minimum of friction-limited and actuator-limited capabilities:

\[ |\dot v + c_v v| \le \min\left(\mu g,\;\frac{2\tau_{max}}{mr}\right). \]

This equation makes the parameter effect explicit: increasing \(m\) or \(r\) reduces achievable acceleration for fixed motors, while decreasing \(\mu\) reduces it for fixed mass and wheels.

A quasi-static tip-over proxy is:

\[ a_{lat} \le g\,\frac{w}{2h}. \]

With \(a_{lat} \approx v|\omega|\), the stability-limited yaw rate is:

\[ |\omega| \le \frac{g}{|v|}\,\frac{w}{2h}. \]

Applied workflow: estimate dominant parameters and uncertainties, compute feasible envelopes, tune low-level controllers for robustness, validate with sweeps, and feed limits to higher layers.

flowchart TD
  S["Start with nominal parameters p0"] --> U["Define uncertainty ranges: p = p0 +/- dp"]
  U --> E["Compute feasibility: traction + stability envelopes"]
  E --> T["Tune low-level control gains for robustness"]
  T --> V["Validate with parameter sweeps and terrain tests"]
  V --> D["Deploy limits to higher layers: accel/turn caps"]
  D --> R["Recalibrate when payload, tires, or surface changes"]
        

Each implementation simulates the same model and performs a parameter sweep (true plant varies; controller stays nominal).

Code: Chapter4_Lesson5.py

#!/usr/bin/env python3
# Chapter4_Lesson5.py
# Parameter Effects on Real Navigation (Applied Dynamics)

from __future__ import annotations

import math
import csv
from dataclasses import dataclass
from typing import Dict, List, Tuple

import numpy as np


@dataclass
class Params:
    m: float
    Iz: float
    r: float
    b: float
    mu: float
    cv: float
    cw: float
    tau_max: float


@dataclass
class Gains:
    kp_v: float
    ki_v: float
    kp_w: float
    ki_w: float


def sat(x: float, lo: float, hi: float) -> float:
    return max(lo, min(hi, x))


def simulate(
    p_nom: Params,
    p_true: Params,
    gains: Gains,
    T: float = 10.0,
    dt: float = 0.01,
    v_ref: float = 0.6,
    w_ref: float = 0.4,
) -> Dict[str, np.ndarray]:
    n = int(T / dt) + 1
    t = np.linspace(0.0, T, n)

    x = np.zeros(n)
    y = np.zeros(n)
    th = np.zeros(n)
    v = np.zeros(n)
    w = np.zeros(n)
    eta_v = np.zeros(n)
    eta_w = np.zeros(n)

    vd = np.full(n, v_ref)
    wd = np.full(n, w_ref)

    for k in range(n - 1):
        ev = vd[k] - v[k]
        ew = wd[k] - w[k]

        F_cmd = p_nom.m * (gains.kp_v * ev + gains.ki_v * eta_v[k]) + p_nom.m * p_nom.cv * v[k]
        M_cmd = p_nom.Iz * (gains.kp_w * ew + gains.ki_w * eta_w[k]) + p_nom.Iz * p_nom.cw * w[k]

        tau_R = 0.5 * (p_nom.r * F_cmd + (2.0 * p_nom.r / p_nom.b) * M_cmd)
        tau_L = 0.5 * (p_nom.r * F_cmd - (2.0 * p_nom.r / p_nom.b) * M_cmd)

        tau_R = sat(tau_R, -p_nom.tau_max, p_nom.tau_max)
        tau_L = sat(tau_L, -p_nom.tau_max, p_nom.tau_max)

        F = (tau_R + tau_L) / max(1e-9, p_true.r)
        M = (p_true.b / (2.0 * max(1e-9, p_true.r))) * (tau_R - tau_L)

        F_max = p_true.mu * p_true.m * 9.81
        F = sat(F, -F_max, F_max)

        M_max = p_true.mu * p_true.m * 9.81 * (p_true.b / 2.0)
        M = sat(M, -M_max, M_max)

        v_dot = (1.0 / p_true.m) * F - p_true.cv * v[k]
        w_dot = (1.0 / p_true.Iz) * M - p_true.cw * w[k]

        x_dot = v[k] * math.cos(th[k])
        y_dot = v[k] * math.sin(th[k])
        th_dot = w[k]

        eta_v_dot = ev
        eta_w_dot = ew

        x[k + 1] = x[k] + dt * x_dot
        y[k + 1] = y[k] + dt * y_dot
        th[k + 1] = th[k] + dt * th_dot
        v[k + 1] = v[k] + dt * v_dot
        w[k + 1] = w[k] + dt * w_dot
        eta_v[k + 1] = eta_v[k] + dt * eta_v_dot
        eta_w[k + 1] = eta_w[k] + dt * eta_w_dot

    return {"t": t, "x": x, "y": y, "th": th, "v": v, "w": w, "vd": vd, "wd": wd}


def l2_pose_error(traj_a: Dict[str, np.ndarray], traj_b: Dict[str, np.ndarray]) -> float:
    dx = traj_a["x"] - traj_b["x"]
    dy = traj_a["y"] - traj_b["y"]
    return float(np.sqrt(np.mean(dx * dx + dy * dy)))


def run_parameter_sweep() -> None:
    p_nom = Params(m=25.0, Iz=2.0, r=0.08, b=0.34, mu=0.7, cv=0.6, cw=0.8, tau_max=3.0)
    gains = Gains(kp_v=4.0, ki_v=1.5, kp_w=3.0, ki_w=1.0)

    base = simulate(p_nom, p_nom, gains, T=10.0, dt=0.01, v_ref=0.6, w_ref=0.4)

    sweep: List[Tuple[str, List[float]]] = [
        ("m",  [0.7, 0.85, 1.0, 1.15, 1.3]),
        ("Iz", [0.7, 0.85, 1.0, 1.15, 1.3]),
        ("r",  [0.95, 0.98, 1.0, 1.02, 1.05]),
        ("b",  [0.95, 0.98, 1.0, 1.02, 1.05]),
        ("mu", [0.25, 0.4, 0.7, 0.9]),
    ]

    rows = []
    for name, factors in sweep:
        for a in factors:
            p_true = Params(**vars(p_nom))
            setattr(p_true, name, getattr(p_nom, name) * a)
            traj = simulate(p_nom, p_true, gains, T=10.0, dt=0.01, v_ref=0.6, w_ref=0.4)
            err = l2_pose_error(base, traj)
            rows.append((name, a, err))

    out_csv = "Chapter4_Lesson5_sweep_summary.csv"
    with open(out_csv, "w", newline="") as f:
        wtr = csv.writer(f)
        wtr.writerow(["parameter", "scale_factor", "l2_pose_error_m"])
        for r in rows:
            wtr.writerow(list(r))

    print("Wrote:", out_csv)


if __name__ == "__main__":
    run_parameter_sweep()
      

Code: Chapter4_Lesson5.cpp

// Chapter4_Lesson5.cpp
// Parameter Effects on Real Navigation (Applied Dynamics)

#include <cmath>
#include <iostream>
#include <vector>
#include <string>
#include <algorithm>

struct Params {
  double m;
  double Iz;
  double r;
  double b;
  double mu;
  double cv;
  double cw;
  double tau_max;
};

struct Gains {
  double kp_v, ki_v;
  double kp_w, ki_w;
};

static inline double sat(double x, double lo, double hi) {
  return std::max(lo, std::min(hi, x));
}

struct Traj {
  std::vector<double> t, x, y, th, v, w, vd, wd;
};

Traj simulate(
  const Params& p_nom,
  const Params& p_true,
  const Gains& g,
  double T,
  double dt,
  double v_ref,
  double w_ref
) {
  int n = static_cast<int>(T / dt) + 1;
  Traj tr;
  tr.t.resize(n); tr.x.assign(n,0.0); tr.y.assign(n,0.0); tr.th.assign(n,0.0);
  tr.v.assign(n,0.0); tr.w.assign(n,0.0);
  tr.vd.assign(n, v_ref); tr.wd.assign(n, w_ref);

  std::vector<double> eta_v(n,0.0), eta_w(n,0.0);

  for (int k = 0; k < n; ++k) tr.t[k] = k * dt;

  for (int k = 0; k < n-1; ++k) {
    double ev = tr.vd[k] - tr.v[k];
    double ew = tr.wd[k] - tr.w[k];

    double F_cmd = p_nom.m * (g.kp_v * ev + g.ki_v * eta_v[k]) + p_nom.m * p_nom.cv * tr.v[k];
    double M_cmd = p_nom.Iz * (g.kp_w * ew + g.ki_w * eta_w[k]) + p_nom.Iz * p_nom.cw * tr.w[k];

    double tau_R = 0.5 * (p_nom.r * F_cmd + (2.0 * p_nom.r / p_nom.b) * M_cmd);
    double tau_L = 0.5 * (p_nom.r * F_cmd - (2.0 * p_nom.r / p_nom.b) * M_cmd);

    tau_R = sat(tau_R, -p_nom.tau_max, p_nom.tau_max);
    tau_L = sat(tau_L, -p_nom.tau_max, p_nom.tau_max);

    double r_true = std::max(1e-9, p_true.r);
    double F = (tau_R + tau_L) / r_true;
    double M = (p_true.b / (2.0 * r_true)) * (tau_R - tau_L);

    double Fmax = p_true.mu * p_true.m * 9.81;
    F = sat(F, -Fmax, Fmax);

    double Mmax = p_true.mu * p_true.m * 9.81 * (p_true.b / 2.0);
    M = sat(M, -Mmax, Mmax);

    double v_dot = (1.0 / p_true.m) * F - p_true.cv * tr.v[k];
    double w_dot = (1.0 / p_true.Iz) * M - p_true.cw * tr.w[k];

    double x_dot  = tr.v[k] * std::cos(tr.th[k]);
    double y_dot  = tr.v[k] * std::sin(tr.th[k]);
    double th_dot = tr.w[k];

    double eta_v_dot = ev;
    double eta_w_dot = ew;

    tr.x[k+1]  = tr.x[k]  + dt * x_dot;
    tr.y[k+1]  = tr.y[k]  + dt * y_dot;
    tr.th[k+1] = tr.th[k] + dt * th_dot;
    tr.v[k+1]  = tr.v[k]  + dt * v_dot;
    tr.w[k+1]  = tr.w[k]  + dt * w_dot;
    eta_v[k+1] = eta_v[k] + dt * eta_v_dot;
    eta_w[k+1] = eta_w[k] + dt * eta_w_dot;
  }

  return tr;
}

double l2_pose_error(const Traj& a, const Traj& b) {
  double acc = 0.0;
  int n = (int)a.x.size();
  for (int k = 0; k < n; ++k) {
    double dx = a.x[k] - b.x[k];
    double dy = a.y[k] - b.y[k];
    acc += dx*dx + dy*dy;
  }
  return std::sqrt(acc / (double)n);
}

int main() {
  Params p_nom{25.0, 2.0, 0.08, 0.34, 0.7, 0.6, 0.8, 3.0};
  Gains g{4.0, 1.5, 3.0, 1.0};

  Traj base = simulate(p_nom, p_nom, g, 10.0, 0.01, 0.6, 0.4);

  struct Row { std::string name; double scale; double err; };
  std::vector<Row> rows;

  auto push_sweep = [&](const std::string& name, const std::vector<double>& scales) {
    for (double s : scales) {
      Params p_true = p_nom;
      if (name == "m") p_true.m *= s;
      else if (name == "Iz") p_true.Iz *= s;
      else if (name == "r") p_true.r *= s;
      else if (name == "b") p_true.b *= s;
      else if (name == "mu") p_true.mu *= s;

      Traj tr = simulate(p_nom, p_true, g, 10.0, 0.01, 0.6, 0.4);
      rows.push_back(Row{name, s, l2_pose_error(base, tr)});
    }
  };

  push_sweep("m",  {0.7, 0.85, 1.0, 1.15, 1.3});
  push_sweep("Iz", {0.7, 0.85, 1.0, 1.15, 1.3});
  push_sweep("r",  {0.95, 0.98, 1.0, 1.02, 1.05});
  push_sweep("b",  {0.95, 0.98, 1.0, 1.02, 1.05});
  push_sweep("mu", {0.25, 0.4, 0.7, 0.9});

  std::sort(rows.begin(), rows.end(), [](const Row& a, const Row& b){ return a.err > b.err; });

  std::cout << "Top deviations (parameter, scale, L2 pose error [m]):\n";
  for (int i = 0; i < std::min(10, (int)rows.size()); ++i) {
    std::cout << rows[i].name << ", " << rows[i].scale << ", " << rows[i].err << "\n";
  }

  return 0;
}
      

Code: Chapter4_Lesson5.java

// Chapter4_Lesson5.java
// Parameter Effects on Real Navigation (Applied Dynamics)

import java.util.*;

public class Chapter4_Lesson5 {

  static class Params {
    double m, Iz, r, b, mu, cv, cw, tauMax;
    Params(double m, double Iz, double r, double b, double mu, double cv, double cw, double tauMax) {
      this.m = m; this.Iz = Iz; this.r = r; this.b = b;
      this.mu = mu; this.cv = cv; this.cw = cw; this.tauMax = tauMax;
    }
    Params copy() { return new Params(m, Iz, r, b, mu, cv, cw, tauMax); }
  }

  static class Gains {
    double kpV, kiV, kpW, kiW;
    Gains(double kpV, double kiV, double kpW, double kiW) {
      this.kpV = kpV; this.kiV = kiV; this.kpW = kpW; this.kiW = kiW;
    }
  }

  static double sat(double x, double lo, double hi) {
    return Math.max(lo, Math.min(hi, x));
  }

  static class Traj {
    double[] x, y, th, v, w;
    int n;
    Traj(int n) {
      this.n = n;
      x = new double[n]; y = new double[n]; th = new double[n]; v = new double[n]; w = new double[n];
    }
  }

  static Traj simulate(Params pNom, Params pTrue, Gains g, double T, double dt, double vRef, double wRef) {
    int n = (int)(T / dt) + 1;
    Traj tr = new Traj(n);
    double[] etaV = new double[n];
    double[] etaW = new double[n];

    for (int k = 0; k < n - 1; k++) {
      double ev = vRef - tr.v[k];
      double ew = wRef - tr.w[k];

      double Fcmd = pNom.m * (g.kpV * ev + g.kiV * etaV[k]) + pNom.m * pNom.cv * tr.v[k];
      double Mcmd = pNom.Iz * (g.kpW * ew + g.kiW * etaW[k]) + pNom.Iz * pNom.cw * tr.w[k];

      double tauR = 0.5 * (pNom.r * Fcmd + (2.0 * pNom.r / pNom.b) * Mcmd);
      double tauL = 0.5 * (pNom.r * Fcmd - (2.0 * pNom.r / pNom.b) * Mcmd);

      tauR = sat(tauR, -pNom.tauMax, pNom.tauMax);
      tauL = sat(tauL, -pNom.tauMax, pNom.tauMax);

      double rTrue = Math.max(1e-9, pTrue.r);
      double F = (tauR + tauL) / rTrue;
      double M = (pTrue.b / (2.0 * rTrue)) * (tauR - tauL);

      double Fmax = pTrue.mu * pTrue.m * 9.81;
      F = sat(F, -Fmax, Fmax);

      double Mmax = pTrue.mu * pTrue.m * 9.81 * (pTrue.b / 2.0);
      M = sat(M, -Mmax, Mmax);

      double vDot = (1.0 / pTrue.m) * F - pTrue.cv * tr.v[k];
      double wDot = (1.0 / pTrue.Iz) * M - pTrue.cw * tr.w[k];

      double xDot = tr.v[k] * Math.cos(tr.th[k]);
      double yDot = tr.v[k] * Math.sin(tr.th[k]);
      double thDot = tr.w[k];

      double etaVDot = ev;
      double etaWDot = ew;

      tr.x[k + 1] = tr.x[k] + dt * xDot;
      tr.y[k + 1] = tr.y[k] + dt * yDot;
      tr.th[k + 1] = tr.th[k] + dt * thDot;
      tr.v[k + 1] = tr.v[k] + dt * vDot;
      tr.w[k + 1] = tr.w[k] + dt * wDot;
      etaV[k + 1] = etaV[k] + dt * etaVDot;
      etaW[k + 1] = etaW[k] + dt * etaWDot;
    }

    return tr;
  }

  static double l2PoseError(Traj a, Traj b) {
    double acc = 0.0;
    for (int k = 0; k < a.n; k++) {
      double dx = a.x[k] - b.x[k];
      double dy = a.y[k] - b.y[k];
      acc += dx * dx + dy * dy;
    }
    return Math.sqrt(acc / a.n);
  }

  static class Row {
    String name;
    double scale, err;
    Row(String name, double scale, double err) { this.name = name; this.scale = scale; this.err = err; }
  }

  public static void main(String[] args) {
    Params pNom = new Params(25.0, 2.0, 0.08, 0.34, 0.7, 0.6, 0.8, 3.0);
    Gains g = new Gains(4.0, 1.5, 3.0, 1.0);

    Traj base = simulate(pNom, pNom, g, 10.0, 0.01, 0.6, 0.4);

    List<Row> rows = new ArrayList<>();

    String[] names = {"m", "Iz", "r", "b", "mu"};
    double[][] scales = {
      {0.7, 0.85, 1.0, 1.15, 1.3},
      {0.7, 0.85, 1.0, 1.15, 1.3},
      {0.95, 0.98, 1.0, 1.02, 1.05},
      {0.95, 0.98, 1.0, 1.02, 1.05},
      {0.25, 0.4, 0.7, 0.9}
    };

    for (int i = 0; i < names.length; i++) {
      for (double s : scales[i]) {
        Params pTrue = pNom.copy();
        if (names[i].equals("m")) pTrue.m *= s;
        if (names[i].equals("Iz")) pTrue.Iz *= s;
        if (names[i].equals("r")) pTrue.r *= s;
        if (names[i].equals("b")) pTrue.b *= s;
        if (names[i].equals("mu")) pTrue.mu *= s;

        Traj tr = simulate(pNom, pTrue, g, 10.0, 0.01, 0.6, 0.4);
        rows.add(new Row(names[i], s, l2PoseError(base, tr)));
      }
    }

    rows.sort((a, b) -> Double.compare(b.err, a.err));

    System.out.println("Top deviations (parameter, scale, L2 pose error [m]):");
    for (int i = 0; i < Math.min(10, rows.size()); i++) {
      Row r = rows.get(i);
      System.out.println(r.name + ", " + r.scale + ", " + r.err);
    }
  }
}
      

Code: Chapter4_Lesson5.m

% Chapter4_Lesson5.m
% Parameter Effects on Real Navigation (Applied Dynamics)

function Chapter4_Lesson5()
  p_nom.m = 25.0;
  p_nom.Iz = 2.0;
  p_nom.r = 0.08;
  p_nom.b = 0.34;
  p_nom.mu = 0.7;
  p_nom.cv = 0.6;
  p_nom.cw = 0.8;
  p_nom.tau_max = 3.0;

  gains.kp_v = 4.0;
  gains.ki_v = 1.5;
  gains.kp_w = 3.0;
  gains.ki_w = 1.0;

  T = 10.0; dt = 0.01;
  v_ref = 0.6; w_ref = 0.4;

  base = simulate(p_nom, p_nom, gains, T, dt, v_ref, w_ref);

  sweep = {
    'm',  [0.7 0.85 1.0 1.15 1.3];
    'Iz', [0.7 0.85 1.0 1.15 1.3];
    'r',  [0.95 0.98 1.0 1.02 1.05];
    'b',  [0.95 0.98 1.0 1.02 1.05];
    'mu', [0.25 0.4 0.7 0.9];
  };

  rows = {};
  idx = 1;

  for s = 1:size(sweep,1)
    name = sweep{s,1};
    factors = sweep{s,2};
    for a = factors
      p_true = p_nom;
      p_true.(name) = p_nom.(name) * a;
      tr = simulate(p_nom, p_true, gains, T, dt, v_ref, w_ref);
      err = l2_pose_error(base, tr);
      rows{idx,1} = name; %#ok<AGROW>
      rows{idx,2} = a;
      rows{idx,3} = err;
      idx = idx + 1;
    end
  end

  out_csv = 'Chapter4_Lesson5_sweep_summary.csv';
  fid = fopen(out_csv, 'w');
  fprintf(fid, 'parameter,scale_factor,l2_pose_error_m\n');
  for i = 1:size(rows,1)
    fprintf(fid, '%s,%.6f,%.9f\n', rows{i,1}, rows{i,2}, rows{i,3});
  end
  fclose(fid);

  disp(['Wrote: ', out_csv]);
end

function tr = simulate(p_nom, p_true, g, T, dt, v_ref, w_ref)
  n = floor(T/dt) + 1;
  x = zeros(n,1); y = zeros(n,1); th = zeros(n,1);
  v = zeros(n,1); w = zeros(n,1);
  eta_v = zeros(n,1); eta_w = zeros(n,1);

  for k = 1:n-1
    ev = v_ref - v(k);
    ew = w_ref - w(k);

    F_cmd = p_nom.m * (g.kp_v*ev + g.ki_v*eta_v(k)) + p_nom.m * p_nom.cv * v(k);
    M_cmd = p_nom.Iz * (g.kp_w*ew + g.ki_w*eta_w(k)) + p_nom.Iz * p_nom.cw * w(k);

    tau_R = 0.5 * (p_nom.r * F_cmd + (2.0*p_nom.r/p_nom.b)*M_cmd);
    tau_L = 0.5 * (p_nom.r * F_cmd - (2.0*p_nom.r/p_nom.b)*M_cmd);

    tau_R = sat(tau_R, -p_nom.tau_max, p_nom.tau_max);
    tau_L = sat(tau_L, -p_nom.tau_max, p_nom.tau_max);

    r_true = max(1e-9, p_true.r);
    F = (tau_R + tau_L) / r_true;
    M = (p_true.b / (2.0*r_true)) * (tau_R - tau_L);

    Fmax = p_true.mu * p_true.m * 9.81;
    F = sat(F, -Fmax, Fmax);

    Mmax = p_true.mu * p_true.m * 9.81 * (p_true.b/2.0);
    M = sat(M, -Mmax, Mmax);

    v_dot = (1.0/p_true.m) * F - p_true.cv * v(k);
    w_dot = (1.0/p_true.Iz) * M - p_true.cw * w(k);

    x_dot = v(k) * cos(th(k));
    y_dot = v(k) * sin(th(k));
    th_dot = w(k);

    x(k+1) = x(k) + dt*x_dot;
    y(k+1) = y(k) + dt*y_dot;
    th(k+1) = th(k) + dt*th_dot;
    v(k+1) = v(k) + dt*v_dot;
    w(k+1) = w(k) + dt*w_dot;
    eta_v(k+1) = eta_v(k) + dt*ev;
    eta_w(k+1) = eta_w(k) + dt*ew;
  end

  tr.x = x; tr.y = y; tr.th = th; tr.v = v; tr.w = w;
end

function y = sat(x, lo, hi)
  y = max(lo, min(hi, x));
end

function err = l2_pose_error(a, b)
  dx = a.x - b.x; dy = a.y - b.y;
  err = sqrt(mean(dx.^2 + dy.^2));
end
      

Code: Chapter4_Lesson5.nb

(* Chapter4_Lesson5.nb
   Parameter Effects on Real Navigation (Applied Dynamics) *)

ClearAll["Global`*"];

sat[x_, lo_, hi_] := Max[lo, Min[hi, x]];

simulate[pNom_Association, pTrue_Association, gains_Association,
  T_: 10.0, dt_: 0.01, vRef_: 0.6, wRef_: 0.4] :=
 Module[{n, t, x, y, th, v, w, etaV, etaW, k, ev, ew, Fcmd, Mcmd, tauR, tauL, rTrue,
    F, M, Fmax, Mmax, vdot, wdot, xdot, ydot, thdot},

  n = Floor[T/dt] + 1;
  t = Range[0, T, dt];
  x = ConstantArray[0.0, n];
  y = ConstantArray[0.0, n];
  th = ConstantArray[0.0, n];
  v = ConstantArray[0.0, n];
  w = ConstantArray[0.0, n];
  etaV = ConstantArray[0.0, n];
  etaW = ConstantArray[0.0, n];

  For[k = 1, k <= n - 1, k++,
   ev = vRef - v[[k]];
   ew = wRef - w[[k]];

   Fcmd = pNom["m"]*(gains["kpV"]*ev + gains["kiV"]*etaV[[k]]) + pNom["m"]*pNom["cv"]*v[[k]];
   Mcmd = pNom["Iz"]*(gains["kpW"]*ew + gains["kiW"]*etaW[[k]]) + pNom["Iz"]*pNom["cw"]*w[[k]];

   tauR = 0.5*(pNom["r"]*Fcmd + (2.0*pNom["r"]/pNom["b"])*Mcmd);
   tauL = 0.5*(pNom["r"]*Fcmd - (2.0*pNom["r"]/pNom["b"])*Mcmd);

   tauR = sat[tauR, -pNom["tauMax"], pNom["tauMax"]];
   tauL = sat[tauL, -pNom["tauMax"], pNom["tauMax"]];

   rTrue = Max[1.0*^-9, pTrue["r"]];
   F = (tauR + tauL)/rTrue;
   M = (pTrue["b"]/(2.0*rTrue))*(tauR - tauL);

   Fmax = pTrue["mu"]*pTrue["m"]*9.81;
   F = sat[F, -Fmax, Fmax];

   Mmax = pTrue["mu"]*pTrue["m"]*9.81*(pTrue["b"]/2.0);
   M = sat[M, -Mmax, Mmax];

   vdot = (1.0/pTrue["m"])*F - pTrue["cv"]*v[[k]];
   wdot = (1.0/pTrue["Iz"])*M - pTrue["cw"]*w[[k]];

   xdot = v[[k]]*Cos[th[[k]]];
   ydot = v[[k]]*Sin[th[[k]]];
   thdot = w[[k]];

   x[[k + 1]] = x[[k]] + dt*xdot;
   y[[k + 1]] = y[[k]] + dt*ydot;
   th[[k + 1]] = th[[k]] + dt*thdot;
   v[[k + 1]] = v[[k]] + dt*vdot;
   w[[k + 1]] = w[[k]] + dt*wdot;
   etaV[[k + 1]] = etaV[[k]] + dt*ev;
   etaW[[k + 1]] = etaW[[k]] + dt*ew;
   ];

  <|"t" -> t, "x" -> x, "y" -> y, "th" -> th, "v" -> v, "w" -> w|>
  ];

l2PoseError[trajA_Association, trajB_Association] :=
 Module[{dx, dy},
  dx = trajA["x"] - trajB["x"];
  dy = trajA["y"] - trajB["y"];
  Sqrt[Mean[dx^2 + dy^2]]
  ];

pNom = <|"m" -> 25.0, "Iz" -> 2.0, "r" -> 0.08, "b" -> 0.34, "mu" -> 0.7,
   "cv" -> 0.6, "cw" -> 0.8, "tauMax" -> 3.0|>;

gains = <|"kpV" -> 4.0, "kiV" -> 1.5, "kpW" -> 3.0, "kiW" -> 1.0|>;

base = simulate[pNom, pNom, gains, 10.0, 0.01, 0.6, 0.4];

sweeps = {
   {"m", {0.7, 0.85, 1.0, 1.15, 1.3}},
   {"Iz", {0.7, 0.85, 1.0, 1.15, 1.3}},
   {"r", {0.95, 0.98, 1.0, 1.02, 1.05}},
   {"b", {0.95, 0.98, 1.0, 1.02, 1.05}},
   {"mu", {0.25, 0.4, 0.7, 0.9}}
   };

rows = Reap[
     Do[
      Do[
       pTrue = Association[pNom];
       pTrue[param] = pNom[param]*s;
       tr = simulate[pNom, pTrue, gains, 10.0, 0.01, 0.6, 0.4];
       err = l2PoseError[base, tr];
       Sow[{param, s, err}],
       {s, scales}],
      { {param, scales}, sweeps}]
     ][[2, 1]];

sorted = Reverse@SortBy[rows, Last];
Take[sorted, UpTo[10]]
      

Problem 1 (Sensitivity ODE): Consider \(\dot{\mathbf{x}} = \mathbf{f}(\mathbf{x},t,\mathbf{p})\) with sensitivity \(\mathbf{S}(t) := \frac{\partial \mathbf{x}}{\partial \mathbf{p}}\). Derive an ODE for \(\mathbf{S}(t)\).

Solution: Differentiate and apply the chain rule:

\[ \dot{\mathbf{S}}(t) = \frac{\partial \mathbf{f}}{\partial \mathbf{x}}\,\mathbf{S}(t) + \frac{\partial \mathbf{f}}{\partial \mathbf{p}}. \]

Problem 2 (Wheel-radius bias): For straight motion, show that relative distance error is approximately \(\varepsilon_r\) when \(\hat r = r(1+\varepsilon_r)\).

Solution: \(\hat v = (1+\varepsilon_r)v\), so \(\hat L = (1+\varepsilon_r)L\):

\[ \frac{\hat L - L}{L} = \varepsilon_r. \]

Problem 3 (Friction-limited acceleration): With \(c_v=0\), show \(|\dot v| \le \mu g\) and compute \(T_{min}\) to reach \(v_f\) from rest.

Solution: \(|F| \le \mu m g\) and \(\dot v = F/m\), so \(|\dot v| \le \mu g\). Using \(\dot v = \mu g\):

\[ T_{min} = \frac{v_f}{\mu g}. \]

Problem 4 (Stability-limited yaw rate): Using \(a_{lat} \le g\frac{w}{2h}\) and \(a_{lat}\approx v|\omega|\), derive \(\omega_{max}\).

Solution:

\[ \omega_{max} = \frac{g}{|v|}\,\frac{w}{2h}. \]

Parameter effects are not “small details”: they reshape achievable accelerations, turning rates, and systematic drift. We derived first-order sensitivity dynamics and practical bounds, showed how wheel geometry biases create heading drift, and connected friction, actuator limits, and stability parameters to motion feasibility envelopes. These results directly motivate the odometry error characterization work in Chapter 5.