A state is a vector of quantities sufficient to predict future evolution of a system under inputs (to a modeling approximation). For mobile robots, the canonical state includes: \( \text{pose} \), \( \text{velocity} \), and an explicit representation of \( \text{uncertainty} \).

We will write a discrete-time state as \( \mathbf{x}_k \): and input as \( \mathbf{u}_k \):. A generic motion model is:

\[ \mathbf{x}_{k+1} = f(\mathbf{x}_k, \mathbf{u}_k) + \mathbf{w}_k, \qquad \mathbf{w}_k \sim \mathcal{N}(\mathbf{0}, \mathbf{Q}_k). \]

In Chapter 1 we primarily focus on choosing state variables and understanding what they mean. In later chapters we will use these states for estimation, mapping, and planning pipelines.

flowchart TD
  A["Robot and frames"] --> B["Choose state variables"]
  B --> C["Pose (x, y, yaw)"]
  B --> D["Velocity (world or body)"]
  B --> E["Uncertainty (covariance)"]
  C --> F["Motion model x_{k+1} = f(x_k, u_k) + w_k"]
  D --> F
  E --> G["Predict uncertainty P_{k+1}"]
  F --> G
  G --> H["Downstream: estimation and navigation"]
        

Throughout, we assume planar motion unless explicitly noted. This matches most indoor AMRs and many outdoor ground vehicles where roll/pitch are treated as secondary or compensated by attitude estimation.

The planar pose encodes position and heading. In minimal coordinates: \( \mathbf{p} \):

\[ \mathbf{p} = \begin{bmatrix} x \\ y \\ \theta \end{bmatrix}, \qquad (x,y)\in\mathbb{R}^2,\;\theta\in\mathbb{S}^1. \]

A coordinate-free representation uses an element of the planar rigid-body transformation group (often denoted \( SE(2) \) in robotics). Define rotation and translation: \( \mathbf{R}(\theta) \): and \( \mathbf{t} \):

\[ \mathbf{R}(\theta) = \begin{bmatrix} \cos\theta & -\sin\theta \\ \sin\theta & \cos\theta \end{bmatrix}, \qquad \mathbf{t} = \begin{bmatrix} x \\ y \end{bmatrix}. \]

The homogeneous transform \( \mathbf{T} \): is:

\[ \mathbf{T}(\mathbf{p}) = \begin{bmatrix} \mathbf{R}(\theta) & \mathbf{t} \\ \mathbf{0}^\top & 1 \end{bmatrix}. \]

Composition and inverse. If pose \( \mathbf{p}_a \) maps coordinates from frame A to world, and \( \mathbf{p}_b \) maps from frame B to frame A, then the world pose of B is obtained by composition:

\[ \mathbf{T}(\mathbf{p}_{w b}) = \mathbf{T}(\mathbf{p}_{w a}) \,\mathbf{T}(\mathbf{p}_{a b}). \]

The inverse transform is:

\[ \mathbf{T}(\mathbf{p})^{-1} = \begin{bmatrix} \mathbf{R}(\theta)^\top & -\mathbf{R}(\theta)^\top\mathbf{t} \\ \mathbf{0}^\top & 1 \end{bmatrix}. \]

Proposition 1 (Correctness of inverse formula).

Let \( \mathbf{T} = \begin{bmatrix}\mathbf{R} & \mathbf{t}\\ \mathbf{0}^\top & 1\end{bmatrix} \) with \( \mathbf{R}^\top\mathbf{R}=\mathbf{I} \). Then the matrix above is \( \mathbf{T}^{-1} \).

Proof. Multiply \( \mathbf{T}\mathbf{T}^{-1} \):

\[ \mathbf{T}\mathbf{T}^{-1} = \begin{bmatrix} \mathbf{R} & \mathbf{t} \\ \mathbf{0}^\top & 1 \end{bmatrix} \begin{bmatrix} \mathbf{R}^\top & -\mathbf{R}^\top\mathbf{t} \\ \mathbf{0}^\top & 1 \end{bmatrix} = \begin{bmatrix} \mathbf{R}\mathbf{R}^\top & -\mathbf{R}\mathbf{R}^\top\mathbf{t} + \mathbf{t} \\ \mathbf{0}^\top & 1 \end{bmatrix} = \begin{bmatrix} \mathbf{I} & \mathbf{0} \\ \mathbf{0}^\top & 1 \end{bmatrix}. \]

Since \( \mathbf{R}\mathbf{R}^\top=\mathbf{I} \), the result is the identity, hence the formula is correct. ■

Practical note: because \( \theta \in \mathbb{S}^1 \), any minimal representation must handle angle wrapping. A standard wrap operator maps to a principal interval, e.g. \( (-\pi, \pi] \).

Mobile robot velocity can be represented in the world frame or the body frame. The distinction matters: body-frame velocities are often what the drivetrain produces (e.g., forward speed), while world-frame velocities are convenient for global reasoning.

Let the planar body-frame translational velocity be \( \mathbf{v}_b \): and the world-frame translational velocity be \( \mathbf{v}_w \):

\[ \mathbf{v}_b = \begin{bmatrix} v_x \\ v_y \end{bmatrix}, \qquad \mathbf{v}_w = \begin{bmatrix} \dot{x} \\ \dot{y} \end{bmatrix}. \]

The relationship is a rotation by heading:

\[ \mathbf{v}_w = \mathbf{R}(\theta)\,\mathbf{v}_b. \]

Proposition 2 (Frame transformation for velocity).

If a point rigidly attached to the robot has world position \( \mathbf{t} \) and body-frame coordinates constant in time, then its world velocity equals \( \mathbf{R}(\theta)\mathbf{v}_b \).

Proof (rigid-body frame argument). For the robot origin, the world position is \( \mathbf{t} \). Define an infinitesimal body-frame displacement \( d\mathbf{s}_b \). The corresponding world displacement is \( d\mathbf{s}_w = \mathbf{R}(\theta)d\mathbf{s}_b \). Dividing by \( dt \) gives:

\[ \frac{d\mathbf{s}_w}{dt} = \mathbf{R}(\theta)\frac{d\mathbf{s}_b}{dt} \;\;\Rightarrow\;\; \mathbf{v}_w = \mathbf{R}(\theta)\mathbf{v}_b. \]

■

For many ground robots (differential drive, car-like, skid-steer), lateral body velocity is small in nominal modeling, motivating the unicycle form: \( \mathbf{u} = [v,\omega]^\top \):

\[ \dot{x} = v\cos\theta,\qquad \dot{y} = v\sin\theta,\qquad \dot{\theta} = \omega. \]

Even when the true platform is not literally a unicycle, this state representation is a powerful first-order abstraction for modeling, estimation, and navigation interfaces.

Real mobile robots do not know their state exactly. We model the pose as a random vector: \( \mathbf{X} \): with mean \( \boldsymbol{\mu} \): and covariance \( \mathbf{P} \):

\[ \boldsymbol{\mu} = \mathbb{E}[\mathbf{X}], \qquad \mathbf{P} = \mathbb{E}\!\left[(\mathbf{X}-\boldsymbol{\mu})(\mathbf{X}-\boldsymbol{\mu})^\top\right]. \]

Covariance summarizes second-order uncertainty and cross-correlation between state components. For planar pose, a common approximation is: \( \mathbf{P}\in\mathbb{R}^{3\times 3} \):

\[ \mathbf{P} = \begin{bmatrix} \sigma_x^2 & \sigma_{xy} & \sigma_{x\theta} \\ \sigma_{xy} & \sigma_y^2 & \sigma_{y\theta} \\ \sigma_{x\theta} & \sigma_{y\theta} & \sigma_\theta^2 \end{bmatrix}. \]

Proposition 3 (Covariance is positive semidefinite).

For any random vector \( \mathbf{X} \) with finite second moments, its covariance matrix \( \mathbf{P} \) satisfies: \( \mathbf{a}^\top \mathbf{P}\mathbf{a} \ge 0 \) for all \( \mathbf{a} \).

Proof. Let \( \mathbf{Y} = \mathbf{a}^\top(\mathbf{X}-\boldsymbol{\mu}) \). Then \( \mathbf{Y} \) is a scalar random variable with variance:

\[ \operatorname{Var}(\mathbf{Y}) = \mathbb{E}[\mathbf{Y}^2] = \mathbb{E}\!\left[\mathbf{a}^\top(\mathbf{X}-\boldsymbol{\mu})(\mathbf{X}-\boldsymbol{\mu})^\top\mathbf{a}\right] = \mathbf{a}^\top \mathbf{P}\mathbf{a}. \]

Since variance is always nonnegative, \( \mathbf{a}^\top \mathbf{P}\mathbf{a} \ge 0 \) for all \( \mathbf{a} \), hence \( \mathbf{P} \) is positive semidefinite. ■

Important modeling caution: \( \theta \) is periodic, so representing heading uncertainty by a Gaussian on \( \mathbb{R} \) is a local approximation. It is accurate when \( \sigma_\theta \) is small relative to \( 2\pi \), and becomes problematic for multi-modal or high-variance headings (treated later in particle filtering).

Suppose the state update is a nonlinear function with additive noise: \( \mathbf{x}_{k+1} = f(\mathbf{x}_k, \mathbf{u}_k) + \mathbf{w}_k \): and \( \mathbb{E}[\mathbf{w}_k]=\mathbf{0} \), \( \operatorname{Cov}(\mathbf{w}_k)=\mathbf{Q}_k \).

Linearize around the mean \( \boldsymbol{\mu}_k \). Define Jacobians: \( \mathbf{F}_k \): and \( \mathbf{G}_k \):

\[ \mathbf{F}_k = \left.\frac{\partial f}{\partial \mathbf{x}}\right|_{(\boldsymbol{\mu}_k,\mathbf{u}_k)}, \qquad \mathbf{G}_k = \left.\frac{\partial f}{\partial \mathbf{w}}\right|_{(\boldsymbol{\mu}_k,\mathbf{u}_k)}. \]

Proposition 4 (First-order covariance propagation).

Under first-order approximation, the predicted covariance is:

\[ \mathbf{P}_{k+1} \approx \mathbf{F}_k \mathbf{P}_k \mathbf{F}_k^\top + \mathbf{G}_k \mathbf{Q}_k \mathbf{G}_k^\top. \]

Proof (Taylor expansion). Let \( \delta\mathbf{x}_k = \mathbf{x}_k - \boldsymbol{\mu}_k \). First-order expansion gives:

\[ \mathbf{x}_{k+1} \approx f(\boldsymbol{\mu}_k,\mathbf{u}_k) + \mathbf{F}_k\,\delta\mathbf{x}_k + \mathbf{G}_k\,\mathbf{w}_k. \]

Subtract the mean and compute covariance using linearity and independence of \( \delta\mathbf{x}_k \) and \( \mathbf{w}_k \):

\[ \mathbf{P}_{k+1} \approx \mathbb{E}\!\left[(\mathbf{F}_k\delta\mathbf{x}_k + \mathbf{G}_k\mathbf{w}_k)(\mathbf{F}_k\delta\mathbf{x}_k + \mathbf{G}_k\mathbf{w}_k)^\top\right] = \mathbf{F}_k\mathbf{P}_k\mathbf{F}_k^\top + \mathbf{G}_k\mathbf{Q}_k\mathbf{G}_k^\top. \]

■

Applied to Euler-discretized unicycle. Using timestep \( \Delta t \) and input \( \mathbf{u}_k = [v_k,\omega_k]^\top \), define:

\[ f(\mathbf{x}_k,\mathbf{u}_k) = \begin{bmatrix} x_k + \Delta t\, v_k \cos\theta_k \\ y_k + \Delta t\, v_k \sin\theta_k \\ \theta_k + \Delta t\, \omega_k \end{bmatrix}. \]

If we treat uncertainty as coming from noisy inputs (a common dead-reckoning view), let the “noise” be \( \delta\mathbf{u}_k \) with covariance \( \mathbf{Q}_{u,k} \), and write: \( \mathbf{x}_{k+1} = f(\mathbf{x}_k,\mathbf{u}_k + \delta\mathbf{u}_k) \). First-order propagation becomes:

\[ \mathbf{P}_{k+1} \approx \mathbf{F}_k \mathbf{P}_k \mathbf{F}_k^\top + \mathbf{G}_{u,k} \mathbf{Q}_{u,k} \mathbf{G}_{u,k}^\top, \]

where \( \mathbf{G}_{u,k} = \frac{\partial f}{\partial \mathbf{u}} \). Computing Jacobians:

\[ \mathbf{F}_k = \begin{bmatrix} 1 & 0 & -\Delta t\,v_k\sin\theta_k \\ 0 & 1 & \Delta t\,v_k\cos\theta_k \\ 0 & 0 & 1 \end{bmatrix}, \qquad \mathbf{G}_{u,k} = \begin{bmatrix} \Delta t\cos\theta_k & 0 \\ \Delta t\sin\theta_k & 0 \\ 0 & \Delta t \end{bmatrix}. \]

flowchart TD
  A["Inputs: v_k, omega_k"] --> B["Integrate pose mean"]
  B --> C["Linearize about mean"]
  C --> D["Compute F_k and G_u,k"]
  D --> E["Update covariance P_{k+1} = F P F^T + G Q G^T"]
  E --> F["Output: (pose mean, covariance)"]
        

This is the simplest mathematically consistent way to attach uncertainty to dead reckoning. Later, we will augment this idea with measurement updates (Bayes filters) to correct drift.

The following reference implementations compute (i) pose integration for a planar robot and (ii) first-order covariance propagation. They are intentionally minimal and suitable as a baseline for later labs.

6.1 Python (NumPy)

Libraries commonly used in AMR research stacks include NumPy for linear algebra. In later chapters, you will also see common robotics ecosystems such as ROS 2 (Python), but for now we keep dependencies minimal.

# Chapter1_Lesson2.py
# Autonomous Mobile Robots — Chapter 1, Lesson 2
# State variables for mobile robots (pose, velocity, uncertainty)
#
# Dependencies:
#   numpy

from dataclasses import dataclass
import math
import numpy as np


def wrap_angle(theta):
    """
    Wrap an angle to (-pi, pi].
    """
    return (theta + math.pi) % (2.0 * math.pi) - math.pi


def dt_is_nonpositive(dt):
    """
    Return True if dt is non-positive, without using less-than or greater-than operators.
    """
    return (dt == 0.0) or (dt != abs(dt))


@dataclass
class Pose2D:
    x: float
    y: float
    theta: float

    def as_vector(self):
        return np.array([self.x, self.y, self.theta], dtype=float)


def pose_to_T(p):
    """
    Homogeneous transform in SE(2).
    """
    c = math.cos(p.theta)
    s = math.sin(p.theta)
    return np.array([[c, -s, p.x],
                     [s,  c, p.y],
                     [0.0, 0.0, 1.0]], dtype=float)


def integrate_unicycle(p, v, omega, dt, method="midpoint"):
    """
    Planar kinematics:
        xdot = v cos(theta)
        ydot = v sin(theta)
        thetadot = omega
    """
    if dt_is_nonpositive(dt):
        raise ValueError("dt must be positive")

    if method == "euler":
        x_next = p.x + dt * v * math.cos(p.theta)
        y_next = p.y + dt * v * math.sin(p.theta)
        th_next = wrap_angle(p.theta + dt * omega)
        return Pose2D(x_next, y_next, th_next)

    if method == "midpoint":
        th_mid = p.theta + 0.5 * dt * omega
        x_next = p.x + dt * v * math.cos(th_mid)
        y_next = p.y + dt * v * math.sin(th_mid)
        th_next = wrap_angle(p.theta + dt * omega)
        return Pose2D(x_next, y_next, th_next)

    raise ValueError("Unknown method")


def jacobians_euler(p, v, dt):
    """
    Euler-discretized model:
        x_{k+1} = x_k + dt * v * cos(theta_k)
        y_{k+1} = y_k + dt * v * sin(theta_k)
        th_{k+1}= th_k + dt * omega_k

    Returns F (3x3) and G (3x2) for u = [v, omega].
    """
    th = p.theta
    c = math.cos(th)
    s = math.sin(th)

    F = np.eye(3, dtype=float)
    F[0, 2] = -dt * v * s
    F[1, 2] =  dt * v * c

    G = np.zeros((3, 2), dtype=float)
    G[0, 0] = dt * c
    G[1, 0] = dt * s
    G[2, 1] = dt
    return F, G


def propagate_covariance(P, p, v, dt, Q_u):
    """
    First-order covariance propagation:
        P_next = F P F^T + G Q_u G^T
    """
    F, G = jacobians_euler(p, v, dt)
    return F @ P @ F.T + G @ Q_u @ G.T


def demo():
    v = 0.8
    omega = 0.35
    dt = 0.05
    N = 200

    sigma_v = 0.05
    sigma_w = 0.03
    Q_u = np.diag([sigma_v * sigma_v, sigma_w * sigma_w])

    p = Pose2D(0.0, 0.0, 0.0)
    P = np.diag([0.02 * 0.02, 0.02 * 0.02, (2.0 * math.pi / 180.0) ** 2])

    rng = np.random.default_rng(7)

    k = 0
    while k != N:
        v_k = v + rng.normal(0.0, sigma_v)
        w_k = omega + rng.normal(0.0, sigma_w)

        p = integrate_unicycle(p, v_k, w_k, dt, method="midpoint")
        P = propagate_covariance(P, p, v, dt, Q_u)

        k = k + 1

    print("Final pose:", p.as_vector())
    print("Covariance diagonal:", np.diag(P))

    P_sym = 0.5 * (P + P.T)
    eigs = np.linalg.eigvalsh(P_sym)
    print("Eigenvalues of sym(P):", eigs)


if __name__ == "__main__":
    demo()

6.2 C++ (Eigen)

Eigen is widely used in robotics for fast linear algebra in C++ pipelines (including many ROS stacks).

// Chapter1_Lesson2.cpp
// Autonomous Mobile Robots — Chapter 1, Lesson 2
// State variables for mobile robots (pose, velocity, uncertainty)
//
// Dependencies:
//   Eigen (header-only)

#include "cstdio"
#include "cstdlib"
#include "cmath"
#include "Eigen/Dense"

struct Pose2D {
  double x;
  double y;
  double theta;
};

static double absd(double v) { return std::fabs(v); }

static int dtIsNonPositive(double dt) {
  return (dt == 0.0) || (dt != absd(dt));
}

static double wrapAngle(double theta) {
  const double PI = 3.14159265358979323846;
  const double twoPi = 2.0 * PI;
  theta = std::fmod(theta + PI, twoPi);
  if (theta != absd(theta)) theta += twoPi;
  return theta - PI;
}

static Pose2D integrateUnicycleMidpoint(const Pose2D& p, double v, double omega, double dt) {
  if (dtIsNonPositive(dt)) {
    std::fprintf(stderr, "dt must be positive\n");
    std::exit(1);
  }
  const double thMid = p.theta + 0.5 * dt * omega;
  Pose2D out;
  out.x = p.x + dt * v * std::cos(thMid);
  out.y = p.y + dt * v * std::sin(thMid);
  out.theta = wrapAngle(p.theta + dt * omega);
  return out;
}

static void jacobiansEuler(const Pose2D& p, double v, double dt,
                           Eigen::MatrixXd& F, Eigen::MatrixXd& G) {
  const double c = std::cos(p.theta);
  const double s = std::sin(p.theta);

  F = Eigen::MatrixXd::Identity(3, 3);
  F(0, 2) = -dt * v * s;
  F(1, 2) =  dt * v * c;

  G = Eigen::MatrixXd::Zero(3, 2);
  G(0, 0) = dt * c;
  G(1, 0) = dt * s;
  G(2, 1) = dt;
}

static Eigen::MatrixXd propagateCovariance(const Eigen::MatrixXd& P,
                                           const Pose2D& p, double v, double dt,
                                           const Eigen::MatrixXd& Q_u) {
  Eigen::MatrixXd F, G;
  jacobiansEuler(p, v, dt, F, G);
  return F * P * F.transpose() + G * Q_u * G.transpose();
}

int main() {
  const double v = 0.8;
  const double omega = 0.35;
  const double dt = 0.05;
  const int N = 200;

  const double sigmaV = 0.05;
  const double sigmaW = 0.03;

  Eigen::MatrixXd Q_u = Eigen::MatrixXd::Zero(2, 2);
  Q_u(0, 0) = sigmaV * sigmaV;
  Q_u(1, 1) = sigmaW * sigmaW;

  Pose2D p{0.0, 0.0, 0.0};

  Eigen::MatrixXd P = Eigen::MatrixXd::Zero(3, 3);
  P(0, 0) = 0.02 * 0.02;
  P(1, 1) = 0.02 * 0.02;
  const double PI = 3.14159265358979323846;
  P(2, 2) = std::pow(2.0 * PI / 180.0, 2.0);

  int k = 0;
  while (k != N) {
    p = integrateUnicycleMidpoint(p, v, omega, dt);
    P = propagateCovariance(P, p, v, dt, Q_u);
    k = k + 1;
  }

  std::printf("Final pose [x y theta] = %.6f  %.6f  %.6f\n", p.x, p.y, p.theta);
  std::printf("Covariance diagonal = %.6e  %.6e  %.6e\n", P(0,0), P(1,1), P(2,2));
  return 0;
}

6.3 Java (EJML)

EJML provides efficient dense linear algebra in Java. This is useful for teaching, prototyping, and certain embedded pipelines.

// Chapter1_Lesson2.java
// Autonomous Mobile Robots — Chapter 1, Lesson 2
// State variables for mobile robots (pose, velocity, uncertainty)
//
// Dependencies:
//   EJML (Efficient Java Matrix Library)

import org.ejml.simple.SimpleMatrix;

public class Chapter1_Lesson2 {

    static class Pose2D {
        double x, y, theta;
        Pose2D(double x, double y, double theta) { this.x = x; this.y = y; this.theta = theta; }
    }

    static boolean dtIsNonPositive(double dt) {
        return (dt == 0.0) || (dt != Math.abs(dt));
    }

    static double wrapAngle(double theta) {
        double twoPi = 2.0 * Math.PI;
        theta = (theta + Math.PI) % twoPi;
        if (theta != Math.abs(theta)) theta = theta + twoPi;
        return theta - Math.PI;
    }

    static Pose2D integrateUnicycleMidpoint(Pose2D p, double v, double omega, double dt) {
        if (dtIsNonPositive(dt)) throw new IllegalArgumentException("dt must be positive");
        double thMid = p.theta + 0.5 * dt * omega;
        double xNext = p.x + dt * v * Math.cos(thMid);
        double yNext = p.y + dt * v * Math.sin(thMid);
        double thNext = wrapAngle(p.theta + dt * omega);
        return new Pose2D(xNext, yNext, thNext);
    }

    static SimpleMatrix jacobianF(Pose2D p, double v, double dt) {
        double c = Math.cos(p.theta);
        double s = Math.sin(p.theta);
        SimpleMatrix F = SimpleMatrix.identity(3);
        F.set(0, 2, -dt * v * s);
        F.set(1, 2,  dt * v * c);
        return F;
    }

    static SimpleMatrix jacobianG(Pose2D p, double dt) {
        double c = Math.cos(p.theta);
        double s = Math.sin(p.theta);
        SimpleMatrix G = new SimpleMatrix(3, 2);
        G.set(0, 0, dt * c);
        G.set(1, 0, dt * s);
        G.set(2, 1, dt);
        return G;
    }

    static SimpleMatrix propagateCovariance(SimpleMatrix P, Pose2D p, double v, double dt, SimpleMatrix Q_u) {
        SimpleMatrix F = jacobianF(p, v, dt);
        SimpleMatrix G = jacobianG(p, dt);
        return F.mult(P).mult(F.transpose()).plus(G.mult(Q_u).mult(G.transpose()));
    }

    public static void main(String[] args) {
        double v = 0.8, omega = 0.35, dt = 0.05;
        int N = 200;

        double sigmaV = 0.05, sigmaW = 0.03;
        SimpleMatrix Q_u = new SimpleMatrix(2, 2);
        Q_u.set(0, 0, sigmaV * sigmaV);
        Q_u.set(1, 1, sigmaW * sigmaW);

        Pose2D p = new Pose2D(0.0, 0.0, 0.0);

        SimpleMatrix P = new SimpleMatrix(3, 3);
        P.set(0, 0, 0.02 * 0.02);
        P.set(1, 1, 0.02 * 0.02);
        P.set(2, 2, Math.pow(2.0 * Math.PI / 180.0, 2.0));

        int k = 0;
        while (k != N) {
            p = integrateUnicycleMidpoint(p, v, omega, dt);
            P = propagateCovariance(P, p, v, dt, Q_u);
            k = k + 1;
        }

        System.out.println("Final pose [x y theta] = " + p.x + " " + p.y + " " + p.theta);
        System.out.println("Covariance diagonal = " + P.get(0,0) + " " + P.get(1,1) + " " + P.get(2,2));
    }
}

6.4 Matlab

% Chapter1_Lesson2.m
% Autonomous Mobile Robots — Chapter 1, Lesson 2
% State variables for mobile robots (pose, velocity, uncertainty)

clear; clc;

v = 0.8;
omega = 0.35;
dt = 0.05;
N = 200;

sigma_v = 0.05;
sigma_w = 0.03;
Q_u = diag([sigma_v^2, sigma_w^2]);

p = [0; 0; 0];
P = diag([0.02^2, 0.02^2, (2*pi/180)^2]);

for k = 1:N
    p = integrate_unicycle_midpoint(p, v, omega, dt);
    [F, G] = jacobians_euler(p, v, dt);
    P = F*P*F' + G*Q_u*G';
end

disp('Final pose [x; y; theta] =');
disp(p);
disp('Covariance diagonal =');
disp(diag(P).');

function tf = dt_is_nonpositive(dt)
    tf = (dt == 0) || (dt ~= abs(dt));
end

function theta = wrap_angle(theta)
    theta = mod(theta + pi, 2*pi) - pi;
end

function p_next = integrate_unicycle_midpoint(p, v, omega, dt)
    if dt_is_nonpositive(dt)
        error('dt must be positive');
    end
    th = p(3);
    th_mid = th + 0.5*dt*omega;
    x_next = p(1) + dt*v*cos(th_mid);
    y_next = p(2) + dt*v*sin(th_mid);
    th_next = wrap_angle(th + dt*omega);
    p_next = [x_next; y_next; th_next];
end

function [F, G] = jacobians_euler(p, v, dt)
    th = p(3);
    c = cos(th);
    s = sin(th);

    F = eye(3);
    F(1,3) = -dt*v*s;
    F(2,3) =  dt*v*c;

    G = zeros(3,2);
    G(1,1) = dt*c;
    G(2,1) = dt*s;
    G(3,2) = dt;
end

6.5 Matlab/Simulink (model generator)

% Chapter1_Lesson2_Simulink.m
% Autonomous Mobile Robots — Chapter 1, Lesson 2
% Create a Simulink model that integrates planar pose
%   xdot = v*cos(theta), ydot = v*sin(theta), thetadot = omega

clear; clc;

model = 'Chapter1_Lesson2_Simulink';
if bdIsLoaded(model)
    close_system(model, 0);
end
if exist([model '.slx'], 'file')
    delete([model '.slx']);
end

new_system(model);
open_system(model);

set_param(model, 'StopTime', '20');

add_block('simulink/Sources/Constant', [model '/v'], 'Value', '0.8', 'Position', [40 50 90 80]);
add_block('simulink/Sources/Constant', [model '/omega'], 'Value', '0.35', 'Position', [40 150 90 180]);

add_block('simulink/Continuous/Integrator', [model '/Int_x'], 'InitialCondition', '0', 'Position', [520 40 550 70]);
add_block('simulink/Continuous/Integrator', [model '/Int_y'], 'InitialCondition', '0', 'Position', [520 100 550 130]);
add_block('simulink/Continuous/Integrator', [model '/Int_theta'], 'InitialCondition', '0', 'Position', [240 150 270 180]);

add_block('simulink/Math Operations/Trigonometric Function', [model '/cos'], 'Operator', 'cos', 'Position', [300 40 360 70]);
add_block('simulink/Math Operations/Trigonometric Function', [model '/sin'], 'Operator', 'sin', 'Position', [300 100 360 130]);

add_block('simulink/Math Operations/Product', [model '/v_cos'], 'Position', [400 40 460 70]);
add_block('simulink/Math Operations/Product', [model '/v_sin'], 'Position', [400 100 460 130]);

add_block('simulink/Sinks/To Workspace', [model '/x_out'], 'VariableName', 'x_out', 'SaveFormat', 'StructureWithTime', 'Position', [600 40 680 70]);
add_block('simulink/Sinks/To Workspace', [model '/y_out'], 'VariableName', 'y_out', 'SaveFormat', 'StructureWithTime', 'Position', [600 100 680 130]);
add_block('simulink/Sinks/To Workspace', [model '/theta_out'], 'VariableName', 'theta_out', 'SaveFormat', 'StructureWithTime', 'Position', [360 150 460 180]);

add_line(model, 'omega/1', 'Int_theta/1');
add_line(model, 'Int_theta/1', 'cos/1');
add_line(model, 'Int_theta/1', 'sin/1');

add_line(model, 'v/1', 'v_cos/1');
add_line(model, 'cos/1', 'v_cos/2');

add_line(model, 'v/1', 'v_sin/1');
add_line(model, 'sin/1', 'v_sin/2');

add_line(model, 'v_cos/1', 'Int_x/1');
add_line(model, 'v_sin/1', 'Int_y/1');

add_line(model, 'Int_x/1', 'x_out/1');
add_line(model, 'Int_y/1', 'y_out/1');
add_line(model, 'Int_theta/1', 'theta_out/1');

save_system(model);

disp(['Created and saved Simulink model: ', model, '.slx']);
disp('Run: sim(model) then inspect x_out, y_out, theta_out in workspace.');

6.6 Wolfram Mathematica

(* Chapter1_Lesson2.nb *)
Notebook[{
  Cell["Autonomous Mobile Robots — Chapter 1, Lesson 2: State Variables (Pose, Velocity, Uncertainty)", "Title"],
  Cell["Symbolic Jacobians and covariance propagation for a planar unicycle model.", "Text"],

  Cell[BoxData@ToBoxes[
    HoldForm[
      f[{x_, y_, th_}, {v_, om_}, dt_] := {x + dt*v*Cos[th], y + dt*v*Sin[th], th + dt*om}
    ]
  ], "Input"],

  Cell[BoxData@ToBoxes[
    HoldForm[
      F[th_, v_, dt_] := D[f[{x, y, th}, {v, om}, dt], {{x, y, th}}] /. {
        Rule[x, 0], Rule[y, 0], Rule[om, 0]
      };
      G[th_, dt_] := D[f[{x, y, th}, {v, om}, dt], {{v, om}}] /. {
        Rule[x, 0], Rule[y, 0]
      };
    ]
  ], "Input"],

  Cell[BoxData@ToBoxes[
    HoldForm[
      Simplify[F[th, v, dt]]
    ]
  ], "Input"],

  Cell[BoxData@ToBoxes[
    HoldForm[
      Simplify[G[th, dt]]
    ]
  ], "Input"],

  Cell[BoxData@ToBoxes[
    HoldForm[
      Pnext[P_, th_, v_, dt_, Q_] := Module[{FF, GG}, FF = F[th, v, dt]; GG = G[th, dt];
        FF.P.Transpose[FF] + GG.Q.Transpose[GG]
      ]
    ]
  ], "Input"],

  Cell[BoxData@ToBoxes[
    HoldForm[
      th0 = 0.2; v0 = 0.8; dt0 = 0.05;
      P0 = DiagonalMatrix[{0.02^2, 0.02^2, (2*Pi/180)^2}];
      Q0 = DiagonalMatrix[{0.05^2, 0.03^2}];
      P1 = N[Pnext[P0, th0, v0, dt0, Q0]];
      {P1, Eigenvalues[0.5*(P1 + Transpose[P1])]}
    ]
  ], "Input"]
}]

Problem 1 (Pose composition in SE(2)).

Let \( \mathbf{p}_1 = [x_1, y_1, \theta_1]^\top \) and \( \mathbf{p}_2 = [x_2, y_2, \theta_2]^\top \). Using homogeneous transforms, derive the composed pose \( \mathbf{p}_{12} \) such that \( \mathbf{T}(\mathbf{p}_{12}) = \mathbf{T}(\mathbf{p}_1)\mathbf{T}(\mathbf{p}_2) \).

Solution. Using block multiplication:

\[ \mathbf{T}(\mathbf{p}_1)\mathbf{T}(\mathbf{p}_2) = \begin{bmatrix} \mathbf{R}(\theta_1)\mathbf{R}(\theta_2) & \mathbf{t}_1 + \mathbf{R}(\theta_1)\mathbf{t}_2 \\ \mathbf{0}^\top & 1 \end{bmatrix}. \]

Since \( \mathbf{R}(\theta_1)\mathbf{R}(\theta_2)=\mathbf{R}(\theta_1+\theta_2) \), we get:

\[ \theta_{12} = \theta_1 + \theta_2, \qquad \begin{bmatrix} x_{12} \\ y_{12} \end{bmatrix} = \begin{bmatrix} x_1 \\ y_1 \end{bmatrix} + \mathbf{R}(\theta_1)\begin{bmatrix} x_2 \\ y_2 \end{bmatrix}. \]

In expanded scalar form:

\[ x_{12} = x_1 + x_2\cos\theta_1 - y_2\sin\theta_1, \qquad y_{12} = y_1 + x_2\sin\theta_1 + y_2\cos\theta_1. \]

Wrap \( \theta_{12} \) to a principal interval if needed. ■

Problem 2 (Jacobian derivation for Euler unicycle).

Consider the discrete model: \( x_{k+1} = x_k + \Delta t\,v_k\cos\theta_k \), \( y_{k+1} = y_k + \Delta t\,v_k\sin\theta_k \), \( \theta_{k+1} = \theta_k + \Delta t\,\omega_k \). Compute \( \mathbf{F}_k = \frac{\partial f}{\partial \mathbf{x}} \) and \( \mathbf{G}_{u,k}=\frac{\partial f}{\partial \mathbf{u}} \).

Solution. Differentiate each component:

\[ \frac{\partial x_{k+1}}{\partial x_k}=1,\quad \frac{\partial x_{k+1}}{\partial y_k}=0,\quad \frac{\partial x_{k+1}}{\partial \theta_k}=-\Delta t\,v_k\sin\theta_k, \]

\[ \frac{\partial y_{k+1}}{\partial x_k}=0,\quad \frac{\partial y_{k+1}}{\partial y_k}=1,\quad \frac{\partial y_{k+1}}{\partial \theta_k}=\Delta t\,v_k\cos\theta_k, \]

\[ \frac{\partial \theta_{k+1}}{\partial x_k}=0,\quad \frac{\partial \theta_{k+1}}{\partial y_k}=0,\quad \frac{\partial \theta_{k+1}}{\partial \theta_k}=1. \]

Stacking gives \( \mathbf{F}_k \) as in Section 5. For input Jacobian:

\[ \frac{\partial x_{k+1}}{\partial v_k}=\Delta t\cos\theta_k,\quad \frac{\partial x_{k+1}}{\partial \omega_k}=0, \]

\[ \frac{\partial y_{k+1}}{\partial v_k}=\Delta t\sin\theta_k,\quad \frac{\partial y_{k+1}}{\partial \omega_k}=0, \qquad \frac{\partial \theta_{k+1}}{\partial v_k}=0,\quad \frac{\partial \theta_{k+1}}{\partial \omega_k}=\Delta t. \]

Stacking gives \( \mathbf{G}_{u,k} \) as in Section 5. ■

Problem 3 (PSD property of covariance).

Let \( \mathbf{P} = \mathbb{E}[(\mathbf{X}-\boldsymbol{\mu})(\mathbf{X}-\boldsymbol{\mu})^\top] \). Show that all eigenvalues of \( \mathbf{P} \) are nonnegative.

Solution. From Proposition 3, \( \mathbf{a}^\top\mathbf{P}\mathbf{a}\ge 0 \) for all \( \mathbf{a} \). Let \( \mathbf{q} \) be an eigenvector with \( \mathbf{P}\mathbf{q}=\lambda\mathbf{q} \). Then:

\[ 0 \le \mathbf{q}^\top\mathbf{P}\mathbf{q} = \mathbf{q}^\top(\lambda\mathbf{q}) = \lambda \|\mathbf{q}\|^2. \]

Since \( \|\mathbf{q}\|^2 > 0 \) for a nonzero eigenvector, we conclude \( \lambda \ge 0 \). ■

Problem 4 (One-step covariance update).

Given \( \Delta t = 0.1 \), \( v_k=1.0 \), \( \theta_k=0 \), and \( \mathbf{P}_k = \operatorname{diag}(0.01, 0.01, 0.0004) \), \( \mathbf{Q}_{u,k}=\operatorname{diag}(0.04, 0.01) \). Compute \( \mathbf{P}_{k+1} \) using \( \mathbf{P}_{k+1} = \mathbf{F}_k\mathbf{P}_k\mathbf{F}_k^\top + \mathbf{G}_{u,k}\mathbf{Q}_{u,k}\mathbf{G}_{u,k}^\top \).

Solution. With \( \theta_k=0 \), \( \sin\theta_k=0 \), \( \cos\theta_k=1 \). Then:

\[ \mathbf{F}_k = \begin{bmatrix} 1 & 0 & 0 \\ 0 & 1 & \Delta t\,v_k \\ 0 & 0 & 1 \end{bmatrix} = \begin{bmatrix} 1 & 0 & 0 \\ 0 & 1 & 0.1 \\ 0 & 0 & 1 \end{bmatrix}, \quad \mathbf{G}_{u,k} = \begin{bmatrix} \Delta t & 0 \\ 0 & 0 \\ 0 & \Delta t \end{bmatrix} = \begin{bmatrix} 0.1 & 0 \\ 0 & 0 \\ 0 & 0.1 \end{bmatrix}. \]

First term:

\[ \mathbf{F}_k\mathbf{P}_k\mathbf{F}_k^\top = \begin{bmatrix} 0.01 & 0 & 0 \\ 0 & 0.01 + (0.1)^2(0.0004) & 0.1(0.0004) \\ 0 & 0.1(0.0004) & 0.0004 \end{bmatrix} = \begin{bmatrix} 0.01 & 0 & 0 \\ 0 & 0.010004 & 0.00004 \\ 0 & 0.00004 & 0.0004 \end{bmatrix}. \]

Second term:

\[ \mathbf{G}_{u,k}\mathbf{Q}_{u,k}\mathbf{G}_{u,k}^\top = \begin{bmatrix} (0.1)^2(0.04) & 0 & 0 \\ 0 & 0 & 0 \\ 0 & 0 & (0.1)^2(0.01) \end{bmatrix} = \begin{bmatrix} 0.0004 & 0 & 0 \\ 0 & 0 & 0 \\ 0 & 0 & 0.0001 \end{bmatrix}. \]

Summing:

\[ \mathbf{P}_{k+1} = \begin{bmatrix} 0.0104 & 0 & 0 \\ 0 & 0.010004 & 0.00004 \\ 0 & 0.00004 & 0.0005 \end{bmatrix}. \]

■

We defined the core AMR state variables: planar pose, velocity in body and world frames, and uncertainty captured by covariance. We proved key properties (pose inverse correctness, covariance PSD), and derived the first-order covariance propagation used in dead reckoning. These results set up the estimation viewpoint used in probabilistic robotics foundations and localization chapters.