In previous chapters, the robot's state evolution was treated as deterministic:

\[ \mathbf{x}_{t+1} = f(\mathbf{x}_t, \mathbf{u}_t), \qquad \mathbf{x}_t \in \mathbb{R}^n,\; \mathbf{u}_t \in \mathbb{R}^m. \]

Real robots, however, operate with uncertainty. We model state and measurements as random vectors evolving according to

\[ \mathbf{x}_{t+1} = f(\mathbf{x}_t, \mathbf{u}_t, \boldsymbol{\theta}) + \mathbf{w}_t, \qquad \mathbf{y}_t = h(\mathbf{x}_t, \boldsymbol{\theta}) + \mathbf{v}_t, \]

\[ \mathbf{w}_t \sim \mathcal{N}(\mathbf{0}, \mathbf{Q}_t), \quad \mathbf{v}_t \sim \mathcal{N}(\mathbf{0}, \mathbf{R}_t). \]

Here \( \mathbf{w}_t \) and \( \mathbf{v}_t \) are process and measurement noise, and \( \boldsymbol{\theta} \) collects uncertain model parameters (masses, link lengths, friction coefficients, sensor biases, etc.).

Two high-level types of uncertainty are useful:

• Aleatoric uncertainty: intrinsic randomness that cannot be reduced by more data (e.g., thermal noise in sensors). Typically modeled as \( \mathbf{w}_t, \mathbf{v}_t \) with known covariance.
• Epistemic uncertainty: ignorance due to incomplete knowledge (e.g., unknown payload mass, approximate friction model, coarse map). Modeled via random parameters \( \boldsymbol{\theta} \) or discrete hypotheses.

The planner ultimately needs (in later lessons) a distribution \( p(\mathbf{x}_t \mid \mathbf{u}_{0:t-1}, \mathbf{y}_{1:t}) \) rather than a single trajectory. In this lesson we focus on where the uncertainty comes from and how to represent it quantitatively.

flowchart TD
  W["True world"] --> RSTATE["Robot state x_t"]
  RSTATE --> A["Actuation"]
  RSTATE --> S["Sensing"]
  RSTATE --> M["Modeling"]
  RSTATE --> E["Environment"]
  A --> AUNC["Command tracking errors, \nbacklash, slip"]
  S --> SUNC["Sensor noise, bias, \nlimited resolution"]
  M --> MUNC["Unknown parameters, \nunmodeled dynamics"]
  E --> EUNC["Moving objects, \ncontact, map errors"]
  AUNC --> PL["Planning under uncertainty"]
  SUNC --> PL
  MUNC --> PL
  EUNC --> PL
        

Actuators (motors, hydraulic servos, brakes) do not track commanded inputs perfectly. Gear backlash, friction, compliance, delays, and saturation cause tracking error between commanded joint torques or velocities and realized motion.

A general discrete-time state-space model with process noise is

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

For a linear time-varying approximation around a nominal trajectory \( (\bar{\mathbf{x}}_t, \bar{\mathbf{u}}_t) \), we write

\[ \mathbf{x}_{t+1} \approx \bar{\mathbf{x}}_{t+1} + \mathbf{A}_t (\mathbf{x}_t - \bar{\mathbf{x}}_t) + \mathbf{B}_t (\mathbf{u}_t - \bar{\mathbf{u}}_t) + \mathbf{w}_t, \]

\[ \mathbf{A}_t = \left.\frac{\partial f}{\partial \mathbf{x}}\right|_{(\bar{\mathbf{x}}_t,\bar{\mathbf{u}}_t,\boldsymbol{\theta})}, \qquad \mathbf{B}_t = \left.\frac{\partial f}{\partial \mathbf{u}}\right|_{(\bar{\mathbf{x}}_t,\bar{\mathbf{u}}_t,\boldsymbol{\theta})}. \]

If at time \( t \) the state has mean \( \boldsymbol{\mu}_t = \mathbb{E}[\mathbf{x}_t] \) and covariance \( \boldsymbol{\Sigma}_t = \operatorname{Cov}(\mathbf{x}_t) \), and \( \mathbf{w}_t \) is zero-mean, independent of \( \mathbf{x}_t \), then (to first order in the linearization) we have:

\[ \boldsymbol{\mu}_{t+1} \approx f(\boldsymbol{\mu}_t, \mathbf{u}_t, \boldsymbol{\theta}), \]

\[ \boldsymbol{\Sigma}_{t+1} \approx \mathbf{A}_t \boldsymbol{\Sigma}_t \mathbf{A}_t^\top + \mathbf{Q}_t. \]

Proof sketch (covariance propagation):

1. Define the error \( \tilde{\mathbf{x}}_t = \mathbf{x}_t - \boldsymbol{\mu}_t \).
2. Linearize the dynamics: \( \tilde{\mathbf{x}}_{t+1} \approx \mathbf{A}_t \tilde{\mathbf{x}}_t + \mathbf{w}_t \).
3. Take covariance: \( \boldsymbol{\Sigma}_{t+1} = \mathbb{E}[\tilde{\mathbf{x}}_{t+1} \tilde{\mathbf{x}}_{t+1}^\top] \approx \mathbf{A}_t \boldsymbol{\Sigma}_t \mathbf{A}_t^\top + \mathbb{E}[\mathbf{w}_t \mathbf{w}_t^\top] \), since cross-terms vanish when \( \tilde{\mathbf{x}}_t \) and \( \mathbf{w}_t \) are independent and zero-mean.
4. Using \( \mathbb{E}[\mathbf{w}_t \mathbf{w}_t^\top] = \mathbf{Q}_t \) yields the formula.

This propagation of covariance through the motion model is central for later belief-space planning and for assessing the risk of collisions along a planned trajectory.

Sensors (encoders, IMUs, cameras, lidars, force-torque sensors) introduce uncertainty through quantization, bias, drift, mis-calibration, and exogenous noise. We model measurements as

\[ \mathbf{y}_t = h(\mathbf{x}_t, \boldsymbol{\theta}) + \mathbf{v}_t, \qquad \mathbf{v}_t \sim \mathcal{N}(\mathbf{0}, \mathbf{R}_t). \]

Examples:

• Encoder counts: additive quantization noise and missing pulses.
• IMU: bias plus white noise: \( \boldsymbol{\omega}_\text{meas} = \boldsymbol{\omega} + \mathbf{b}_\omega + \mathbf{v}_\omega \).
• Range sensor: \( y = \|\mathbf{p} - \mathbf{p}_\text{obs}\| + v \) with \( v \sim \mathcal{N}(0,\sigma_r^2) \).

For small measurement noise and an approximate Gaussian prior on \( \mathbf{x}_t \), the linearization

\[ \mathbf{y}_t \approx h(\boldsymbol{\mu}_t,\boldsymbol{\theta}) + \mathbf{H}_t (\mathbf{x}_t - \boldsymbol{\mu}_t) + \mathbf{v}_t, \]

\[ \mathbf{H}_t = \left.\frac{\partial h}{\partial \mathbf{x}}\right|_{\mathbf{x}=\boldsymbol{\mu}_t}, \]

yields the predicted measurement distribution

\[ \mathbf{y}_t \sim \mathcal{N}\!\Big( h(\boldsymbol{\mu}_t,\boldsymbol{\theta}), \mathbf{S}_t \Big),\quad \mathbf{S}_t = \mathbf{H}_t \boldsymbol{\Sigma}_t \mathbf{H}_t^\top + \mathbf{R}_t. \]

The matrix \( \mathbf{S}_t \) is the innovation covariance in Bayesian filtering and quantifies how uncertain each measurement is, combining both state uncertainty and sensor noise. Even though full state estimation will be treated later, this measurement uncertainty is already critical for planning safe motions (e.g., how much clearance to leave near obstacles).

So far, \( f \) and \( h \) were assumed known up to additive noise. In practice, kinematic and dynamic models are only approximate:

• Link lengths and offsets known up to calibration error.
• Inertial parameters (mass, centers of mass, inertia tensors) approximated.
• Friction, backlash, and compliance simplified or omitted.

We collect such parameters into \( \boldsymbol{\theta} \) and treat them as random:

\[ \boldsymbol{\theta} \sim p(\boldsymbol{\theta}), \quad \mathbf{x}_{t+1} = f(\mathbf{x}_t,\mathbf{u}_t,\boldsymbol{\theta}) + \mathbf{w}_t. \]

For example, an uncertain link length \( \ell \) with mean \( \bar{\ell} \) and variance \( \sigma_\ell^2 \) induces pose uncertainty even with perfect encoders. Consider a 2D planar arm end-effector position

\[ \mathbf{p}(\ell) = \begin{bmatrix} \ell \cos\phi \\ \ell \sin\phi \end{bmatrix}, \]

where joint angle \( \phi \) is assumed known. Linearizing around \( \bar{\ell} \) gives

\[ \mathbf{p}(\ell) \approx \mathbf{p}(\bar{\ell}) + \frac{\partial \mathbf{p}}{\partial \ell}\bigg|_{\bar{\ell}} (\ell-\bar{\ell}), \qquad \frac{\partial \mathbf{p}}{\partial \ell} = \begin{bmatrix} \cos\phi \\ \sin\phi \end{bmatrix}. \]

Hence the induced covariance of end-effector position is

\[ \operatorname{Cov}(\mathbf{p}) \approx \begin{bmatrix} \cos\phi \\ \sin\phi \end{bmatrix} \begin{bmatrix} \cos\phi & \sin\phi \end{bmatrix} \sigma_\ell^2 = \sigma_\ell^2 \begin{bmatrix} \cos^2\phi & \cos\phi\sin\phi \\ \cos\phi\sin\phi & \sin^2\phi \end{bmatrix}. \]

This shows a typical pattern: parametric uncertainty in kinematics and dynamics propagates to geometric uncertainty in configuration and task space, even if actuation and sensing were noiseless.

In addition to parametric uncertainty, there may be structural model error: the true dynamics do not belong to the chosen model class (e.g., neglected flexible modes, fluid-structure interactions). Structural uncertainty is more difficult to quantify and often represented by larger noise covariances \( \mathbf{Q}_t, \mathbf{R}_t \) or by introducing extra state components.

The robot rarely acts in a perfectly known, static environment. Sources of environment-driven uncertainty include:

• Approximate maps (occupancy grids, meshes) with limited resolution.
• Unknown or moving obstacles (humans, other robots, objects).
• Uncertain contact properties (friction, compliance, sticking vs. sliding).

A common representation in manipulation and mobile robotics is the occupancy grid. For each cell \( i \), we model a Bernoulli random variable:

\[ \Pr(\text{cell } i \text{ occupied}) = p_i, \qquad 0 \le p_i \le 1. \]

Planning with such maps requires reasoning over the joint distribution of occupancies, which rapidly becomes intractable; later lessons will introduce approximations (e.g., chance constraints).

For uncertain obstacle positions, a Gaussian model is often used:

\[ \mathbf{c}_\text{obs} \sim \mathcal{N}(\hat{\mathbf{c}}_\text{obs}, \mathbf{\Sigma}_c), \]

where \( \mathbf{c}_\text{obs} \) is, e.g., the center of a spherical obstacle. If the robot's position is also Gaussian, \( \mathbf{x}_t \sim \mathcal{N}(\boldsymbol{\mu}_t,\boldsymbol{\Sigma}_t) \), then the relative position

\[ \mathbf{r}_t = \mathbf{x}_t - \mathbf{c}_\text{obs} \]

is Gaussian with mean \( \boldsymbol{\mu}_t - \hat{\mathbf{c}}_\text{obs} \) and covariance \( \boldsymbol{\Sigma}_t + \mathbf{\Sigma}_c \), using linearity of expectation and covariance. This simple calculation already shows how environment and state uncertainty couple at the geometric level.

In an autonomous robot, uncertainty propagates around a closed loop: control actions are issued based on uncertain state estimates, those actions produce uncertain motion, and noisy sensors provide partial feedback. At a high level:

flowchart TD
  BEL["Belief over state p(x_t)"] --> CTRL["Controller uses belief, outputs u_t"]
  CTRL --> ACT["Actuators, dynamics with process noise"]
  ACT --> STATE["New state x_{t+1}"]
  STATE --> SENS["Sensors with measurement noise"]
  SENS --> EST["State estimator updates belief"]
  EST --> BEL
        

For the purposes of this first lesson, we focus on the mathematical building blocks of propagation through known maps.

Consider a random vector \( \mathbf{z} \sim \mathcal{N}(\boldsymbol{\mu},\boldsymbol{\Sigma}) \) and a smooth map \( g:\mathbb{R}^n \to \mathbb{R}^k \). Using a first-order Taylor expansion:

\[ g(\mathbf{z}) \approx g(\boldsymbol{\mu}) + \mathbf{G}(\mathbf{z}-\boldsymbol{\mu}), \quad \mathbf{G} = \left.\frac{\partial g}{\partial \mathbf{z}}\right|_{\mathbf{z}=\boldsymbol{\mu}}. \]

Then

\[ \mathbb{E}[g(\mathbf{z})] \approx g(\boldsymbol{\mu}), \qquad \operatorname{Cov}(g(\mathbf{z})) \approx \mathbf{G}\boldsymbol{\Sigma}\mathbf{G}^\top. \]

This is the delta method, used repeatedly in robotics: from forward kinematics, to dynamics, to collision distances. In linear systems, it is exact. For example, if

\[ g(\mathbf{z}) = \mathbf{A}\mathbf{z} + \mathbf{b}, \]

then

\[ \mathbb{E}[g(\mathbf{z})] = \mathbf{A}\boldsymbol{\mu} + \mathbf{b}, \quad \operatorname{Cov}(g(\mathbf{z})) = \mathbf{A}\boldsymbol{\Sigma}\mathbf{A}^\top. \]

Proof: Since \( \mathbf{z} = \boldsymbol{\mu} + \tilde{\mathbf{z}} \) with \( \mathbb{E}[\tilde{\mathbf{z}}] = \mathbf{0} \), we have \( g(\mathbf{z}) = \mathbf{A}\boldsymbol{\mu} + \mathbf{b} + \mathbf{A}\tilde{\mathbf{z}} \). Taking expectation and covariance exploits linearity and the fact that constants do not affect covariance.

These formulas justify representing the robot's uncertainty by mean and covariance (Gaussian approximation) in many planning algorithms, while recognizing that errors may be larger for strongly nonlinear motions or contact events.

We implement a simple stochastic motion model for a point robot in the plane with state \( \mathbf{x}_t = [x_t, y_t, \phi_t]^\top \) and control \( \mathbf{u}_t = [v_t, \omega_t]^\top \) (linear and angular velocity). The (discretized) nominal motion model is

\[ x_{t+1} = x_t + v_t \cos\phi_t \Delta t,\quad y_{t+1} = y_t + v_t \sin\phi_t \Delta t,\quad \phi_{t+1} = \phi_t + \omega_t \Delta t. \]

We inject Gaussian process noise \( \mathbf{w}_t \sim \mathcal{N}(\mathbf{0}, \mathbf{Q}) \) and Gaussian sensor noise on a range-bearing sensor.

7.1 Python (NumPy + robotics toolbox)

import numpy as np

# Time step
dt = 0.1

def f(x, u):
    """
    Stochastic unicycle motion model without noise.
    x: np.array shape (3,), [x, y, phi]
    u: np.array shape (2,), [v, omega]
    """
    px, py, phi = x
    v, omega = u
    return np.array([
        px + v * np.cos(phi) * dt,
        py + v * np.sin(phi) * dt,
        phi + omega * dt
    ])

# Process noise covariance (x, y, phi)
Q = np.diag([0.01**2, 0.01**2, np.deg2rad(1.0)**2])

# Measurement model: range and bearing to a known landmark at (lx, ly)
def h(x, landmark):
    px, py, phi = x
    lx, ly = landmark
    dx = lx - px
    dy = ly - py
    rng = np.hypot(dx, dy)
    bearing = np.arctan2(dy, dx) - phi
    return np.array([rng, bearing])

R = np.diag([0.05**2, np.deg2rad(2.0)**2])

rng = np.random.default_rng(seed=42)

def sample_process_noise():
    return rng.multivariate_normal(mean=np.zeros(3), cov=Q)

def sample_measurement_noise():
    return rng.multivariate_normal(mean=np.zeros(2), cov=R)

# Monte Carlo propagation
N = 10000
x0 = np.array([0.0, 0.0, 0.0])
u = np.array([1.0, 0.2])  # constant command
landmark = np.array([5.0, 0.0])

particles = np.repeat(x0[None, :], N, axis=0)
for t in range(20):
    for i in range(N):
        x_nom = f(particles[i], u)
        w = sample_process_noise()
        particles[i] = x_nom + w

# Empirical mean and covariance of state after 20 steps
mu_hat = particles.mean(axis=0)
Sigma_hat = np.cov(particles.T)

print("Estimated mean:", mu_hat)
print("Estimated covariance:\n", Sigma_hat)

# Sample noisy measurement from one particle
x_sample = particles[0]
y_nom = h(x_sample, landmark)
v = sample_measurement_noise()
y = y_nom + v
print("Noisy measurement (range, bearing):", y)

# Example: using the Robotics Toolbox for Python for more complex robots
# (Peter Corke, roboticstoolbox-python)
# from roboticstoolbox import DHRobot, RevoluteDH
# Define a simple manipulator and sample link parameter uncertainties, etc.
      

7.2 C++ (Eigen, typical in ROS-based robotics)

#include <Eigen/Dense>
#include <random>
#include <iostream>

using Vector3 = Eigen::Vector3d;
using Matrix3 = Eigen::Matrix3d;

// Time step
const double dt = 0.1;

Vector3 f(const Vector3& x, const Eigen::Vector2d& u)
{
    double px = x(0);
    double py = x(1);
    double phi = x(2);
    double v = u(0);
    double omega = u(1);

    Vector3 xn;
    xn(0) = px + v * std::cos(phi) * dt;
    xn(1) = py + v * std::sin(phi) * dt;
    xn(2) = phi + omega * dt;
    return xn;
}

int main()
{
    // Process noise covariance
    Matrix3 Q = Matrix3::Zero();
    Q(0,0) = 0.01 * 0.01;
    Q(1,1) = 0.01 * 0.01;
    Q(2,2) = std::pow(M_PI / 180.0, 2); // 1 deg in rad

    std::mt19937 gen(0);
    std::normal_distribution<double> standard_normal(0.0, 1.0);

    auto sample_process_noise = [&]() {
        Vector3 z;
        for (int i = 0; i < 3; ++i)
            z(i) = standard_normal(gen);
        // Cholesky factor of Q
        Eigen::LLT<Matrix3> llt(Q);
        Matrix3 L = llt.matrixL();
        return L * z;
    };

    Vector3 x;
    x << 0.0, 0.0, 0.0;
    Eigen::Vector2d u;
    u << 1.0, 0.2;

    const int T = 20;
    for (int t = 0; t < T; ++t) {
        Vector3 x_nom = f(x, u);
        Vector3 w = sample_process_noise();
        x = x_nom + w;
    }

    std::cout << "Final stochastic state: " << x.transpose() << std::endl;

    // In a ROS2 or ROS1 stack, such propagation would often be part of a node
    // using Eigen, tf2, and sensor_msgs for message-passing.
    return 0;
}
      

7.3 Java (Apache Commons Math)

import java.util.Random;

public class StochasticUnicycle {

    static final double DT = 0.1;
    static final Random rng = new Random(42L);

    // x = [x, y, phi]
    static double[] f(double[] x, double[] u) {
        double px = x[0];
        double py = x[1];
        double phi = x[2];
        double v = u[0];
        double omega = u[1];

        return new double[] {
            px + v * Math.cos(phi) * DT,
            py + v * Math.sin(phi) * DT,
            phi + omega * DT
        };
    }

    static double[] sampleProcessNoise(double sigmaX, double sigmaY, double sigmaPhi) {
        return new double[] {
            sigmaX * rng.nextGaussian(),
            sigmaY * rng.nextGaussian(),
            sigmaPhi * rng.nextGaussian()
        };
    }

    public static void main(String[] args) {
        double[] x = new double[] {0.0, 0.0, 0.0};
        double[] u = new double[] {1.0, 0.2};

        // Standard deviations of process noise
        double sigmaX = 0.01;
        double sigmaY = 0.01;
        double sigmaPhi = Math.toRadians(1.0);

        for (int t = 0; t < 20; ++t) {
            double[] xNom = f(x, u);
            double[] w = sampleProcessNoise(sigmaX, sigmaY, sigmaPhi);
            for (int i = 0; i < 3; ++i) {
                x[i] = xNom[i] + w[i];
            }
        }

        System.out.printf("Final stochastic state: x=%.3f, y=%.3f, phi=%.3f%n",
                          x[0], x[1], x[2]);
    }
}
      

7.4 MATLAB / Simulink

dt = 0.1;

f = @(x, u) [ ...
    x(1) + u(1)*cos(x(3))*dt; ...
    x(2) + u(1)*sin(x(3))*dt; ...
    x(3) + u(2)*dt];

Q = diag([0.01^2, 0.01^2, deg2rad(1)^2]);

N = 5000;
x0 = [0; 0; 0];
u = [1; 0.2];

X = zeros(3, N);
for k = 1:N
    x = x0;
    for t = 1:20
        x_nom = f(x, u);
        w = mvnrnd([0 0 0], Q).';
        x = x_nom + w;
    end
    X(:,k) = x;
end

mu_hat = mean(X, 2);
Sigma_hat = cov(X.');

disp('Estimated mean:');
disp(mu_hat);
disp('Estimated covariance:');
disp(Sigma_hat);

% Simulink suggestion:
% Build a model where:
%  - A MATLAB Function block implements f(x,u).
%  - Add Random Number blocks for each state component, scaled by sqrt of Q.
%  - Sum them into the state update.
%  - Use Scope blocks to visualize stochastic trajectories.
      

7.5 Wolfram Mathematica

Clear[f, x, u, dt];
dt = 0.1;

(* Symbolic discrete-time dynamics for unicycle *)
f[{x_, y_, phi_}, {v_, omega_}] := {
  x + v Cos[phi] dt,
  y + v Sin[phi] dt,
  phi + omega dt
};

(* Jacobian w.r.t. state *)
F[x_, y_, phi_, v_, omega_] :=
  D[f[{x, y, phi}, {v, omega}], ];

(* Example covariance propagation *)
Q = DiagonalMatrix[{0.01^2, 0.01^2, (Degree)^2}];

mu0 = {0., 0., 0.};
Sigma0 = DiagonalMatrix[{0.0, 0.0, 0.0}];
uval = {1.0, 0.2};

mu = mu0;
Sigma = Sigma0;
Do[
  (* Linearize about current mean *)
  Fval = F[mu[[1]], mu[[2]], mu[[3]], uval[[1]], uval[[2]]];
  mu = f[mu, uval];
  Sigma = Fval.Sigma.Transpose[Fval] + Q;
  ,
  {t, 1, 20}
];

mu // N
Sigma // MatrixForm
      

These implementations illustrate how the same stochastic motion model is represented across typical robotics programming ecosystems: Python with scientific and robotics toolboxes, C++ with Eigen (and often ROS), Java with standard libraries or Apache Commons Math, MATLAB/Simulink, and Mathematica for symbolic and numeric analysis.

Problem 1 (Linear Process Noise Propagation): Consider a linear time-invariant system \( \mathbf{x}_{t+1} = \mathbf{A}\mathbf{x}_t + \mathbf{B}\mathbf{u}_t + \mathbf{w}_t \) with \( \mathbf{w}_t \sim \mathcal{N}(\mathbf{0},\mathbf{Q}) \) i.i.d. Assume \( \mathbf{x}_t \) has mean \( \boldsymbol{\mu}_t \) and covariance \( \boldsymbol{\Sigma}_t \). Derive \( \boldsymbol{\mu}_{t+1} \) and \( \boldsymbol{\Sigma}_{t+1} \).

Solution:

Taking expectation:

\[ \boldsymbol{\mu}_{t+1} = \mathbb{E}[\mathbf{x}_{t+1}] = \mathbf{A}\mathbb{E}[\mathbf{x}_t] + \mathbf{B}\mathbf{u}_t + \mathbb{E}[\mathbf{w}_t] = \mathbf{A}\boldsymbol{\mu}_t + \mathbf{B}\mathbf{u}_t. \]

Define the zero-mean error \( \tilde{\mathbf{x}}_t = \mathbf{x}_t - \boldsymbol{\mu}_t \). Then

\[ \tilde{\mathbf{x}}_{t+1} = \mathbf{x}_{t+1} - \boldsymbol{\mu}_{t+1} = \mathbf{A}\tilde{\mathbf{x}}_t + \mathbf{w}_t. \]

Thus

\[ \begin{aligned} \boldsymbol{\Sigma}_{t+1} &= \mathbb{E}[\tilde{\mathbf{x}}_{t+1}\tilde{\mathbf{x}}_{t+1}^\top] \\ &= \mathbb{E}[(\mathbf{A}\tilde{\mathbf{x}}_t + \mathbf{w}_t) (\mathbf{A}\tilde{\mathbf{x}}_t + \mathbf{w}_t)^\top] \\ &= \mathbf{A}\boldsymbol{\Sigma}_t\mathbf{A}^\top + \mathbb{E}[\mathbf{w}_t \mathbf{w}_t^\top] + \underbrace{\mathbf{A}\mathbb{E}[\tilde{\mathbf{x}}_t\mathbf{w}_t^\top] + \mathbb{E}[\mathbf{w}_t\tilde{\mathbf{x}}_t^\top]\mathbf{A}^\top}_{=0} \\ &= \mathbf{A}\boldsymbol{\Sigma}_t\mathbf{A}^\top + \mathbf{Q}, \end{aligned} \]

using independence and zero-mean of \( \mathbf{w}_t \).

Problem 2 (1D Motion and Sensing): A one-dimensional robot evolves according to \( x_{t+1} = x_t + u_t + w_t \) with \( w_t \sim \mathcal{N}(0,\sigma_w^2) \), and the sensor provides \( y_t = x_t + v_t \) with \( v_t \sim \mathcal{N}(0,\sigma_v^2) \), independent of \( w_t \). Assume \( x_t \sim \mathcal{N}(\mu_t,\sigma_t^2) \). Compute the distribution of \( x_{t+1} \) and of \( y_t \).

Solution:

Since \( x_t \) and \( w_t \) are independent Gaussians, their sum is Gaussian:

\[ x_{t+1} = x_t + u_t + w_t \sim \mathcal{N}(\mu_t + u_t,\; \sigma_t^2 + \sigma_w^2). \]

For the measurement:

\[ y_t = x_t + v_t \sim \mathcal{N}(\mu_t,\; \sigma_t^2 + \sigma_v^2), \]

again using the fact that the sum of independent Gaussian random variables is Gaussian with mean equal to the sum of means and variance equal to the sum of variances.

Problem 3 (Parametric vs Aleatoric Uncertainty): A planar manipulator has an uncertain link length \( \ell \) and encoder noise on joint angles. Classify these uncertainties as aleatoric or epistemic and explain how each type would enter the mathematical model.

Solution:

The encoder noise is typically aleatoric: it arises from quantization, electronics, and thermal noise, and persists even after calibration. It is modeled as additive noise \( \mathbf{v}_t \) in the measurement model with a known covariance \( \mathbf{R}_t \).

The uncertain link length \( \ell \) is epistemic: with improved calibration or identification, the uncertainty could be reduced. It enters as a random parameter in the kinematic model:

\[ \mathbf{p} = f_\text{kin}(\mathbf{q}, \ell), \quad \ell \sim p(\ell), \]

where \( p(\ell) \) may be approximated by a Gaussian prior. One may propagate this uncertainty via linearization as in Section 4.

Problem 4 (Delta Method for a Range Sensor): A robot at uncertain position \( \mathbf{x} = [x,y]^\top \sim \mathcal{N}(\boldsymbol{\mu}, \boldsymbol{\Sigma}) \) measures the range to a known landmark at \( \mathbf{c} = [c_x,c_y]^\top \) via \( r = \|\mathbf{x} - \mathbf{c}\| \). Use first-order linearization to approximate \( \operatorname{Var}(r) \).

Solution:

The measurement function is

\[ h(\mathbf{x}) = \|\mathbf{x} - \mathbf{c}\| = \sqrt{(x-c_x)^2 + (y-c_y)^2}. \]

The gradient at \( \boldsymbol{\mu} \) is

\[ \mathbf{H} = \left.\frac{\partial h}{\partial \mathbf{x}}\right|_{\mathbf{x}=\boldsymbol{\mu}} = \frac{1}{\|\boldsymbol{\mu}-\mathbf{c}\|} \begin{bmatrix} \mu_x - c_x & \mu_y - c_y \end{bmatrix}. \]

The delta method yields

\[ \operatorname{Var}(r) \approx \mathbf{H}\boldsymbol{\Sigma}\mathbf{H}^\top, \]

which is a scalar since \( r \) is one-dimensional. Intuitively, the uncertainty along the radial direction matters most, while tangential uncertainty contributes less to the range variance.

Problem 5 (Coupling of State and Environment Uncertainty): Let \( \mathbf{x} \sim \mathcal{N}(\boldsymbol{\mu}_x,\boldsymbol{\Sigma}_x) \) denote the robot position and \( \mathbf{c} \sim \mathcal{N}(\boldsymbol{\mu}_c,\boldsymbol{\Sigma}_c) \) denote an uncertain obstacle center, independent of \( \mathbf{x} \). Show that the relative position \( \mathbf{r} = \mathbf{x} - \mathbf{c} \) is Gaussian and compute its mean and covariance.

Solution:

Since \( \mathbf{x} \) and \( \mathbf{c} \) are independent Gaussians, any affine combination is Gaussian. We have

\[ \mathbf{r} = \mathbf{x} - \mathbf{c} = \begin{bmatrix} \mathbf{I} & -\mathbf{I} \end{bmatrix} \begin{bmatrix} \mathbf{x} \\ \mathbf{c} \end{bmatrix}, \]

so \( \mathbf{r} \) is Gaussian with mean

\[ \mathbb{E}[\mathbf{r}] = \boldsymbol{\mu}_x - \boldsymbol{\mu}_c \]

and covariance

\[ \operatorname{Cov}(\mathbf{r}) = \boldsymbol{\Sigma}_x + \boldsymbol{\Sigma}_c, \]

since cross-covariances vanish by independence. This demonstrates explicitly how state and map uncertainties add when considered in relative coordinates.

This lesson classified and formalized the main sources of uncertainty in real robots: process (actuation) noise, sensor noise, parametric and structural model error, and environment uncertainty. We introduced stochastic state-space models, Gaussian approximations, and first-order propagation of means and covariances through nonlinear maps via linearization. We also illustrated how to simulate these uncertainties in Python, C++, Java, MATLAB/Simulink, and Mathematica.

Subsequent lessons will build on this foundation to define belief-space planning, chance constraints, and formal frameworks (such as POMDPs) that explicitly incorporate these uncertainties into the planning problem.