In deterministic motion planning, we typically search for a sequence of states \( x_0, x_1, \dots, x_N \) and controls \( u_0, \dots, u_{N-1} \) such that \( x_k \in \mathcal{X}_{\text{free}} \) and \( u_k \in \mathcal{U} \) for all time steps. Under uncertainty, the state \( x_k \) and even obstacle locations become random variables. From Lesson 2 (belief-space planning), we represent uncertainty via a belief \( b_k(x) \), often a Gaussian \( \mathcal{N}(\mu_k, \Sigma_k) \).

Instead of hard constraints on the (unknown) true state, we impose chance constraints that limit the probability of violation. For example, a safety requirement can be written as

\[ \mathbb{P}\big(x_k \in \mathcal{X}_{\text{safe}} \text{ for all } k = 0,\dots,N\big) \; \ge \; 1 - \alpha, \]

where \( \alpha \in (0,1) \) is a risk budget (e.g. \( \alpha = 0.01 \) allows at most 1% probability of collision over the entire horizon).

At a high level, a chance-constrained planner:

• Propagates the belief \( b_k(x) \) forward using the stochastic dynamics.
• Translates probabilistic safety constraints into deterministic inequalities on \( (\mu_k, \Sigma_k) \).
• Searches or optimizes over control sequences while enforcing those inequalities.

flowchart TD
  B0["Belief at k=0: p(x0)"] --> PROP["Propagate with stochastic dynamics"]
  PROP --> DIST["Predict distributions p(xk) along candidate trajectory"]
  DIST --> CC["Evaluate chance constraints: P(violation) <= alpha"]
  CC --> FEAS["Accept controls if constraints satisfied"]
  CC --> INFEAS["Otherwise adjust controls or path"]
  FEAS --> PLAN["Return risk-bounded plan"]
        

In this lesson we stay within the setting of linear dynamics with Gaussian noise, where chance constraints admit analytic reformulations that lead to convex optimization problems. This is a key building block for more complex settings later in the course.

Consider a discrete-time stochastic system

\[ x_{k+1} = f(x_k, u_k, w_k), \quad w_k \sim \mathcal{W}, \]

with state space \( \mathcal{X} \subset \mathbb{R}^n \), control space \( \mathcal{U} \subset \mathbb{R}^m \), and process noise \( w_k \). For this lesson we will later specialize to linear dynamics \( x_{k+1} = A x_k + B u_k + w_k \) with Gaussian noise.

Let \( g_{k,j}(x_k, u_k) \le 0 \) be constraint functions encoding collision avoidance, actuator limits, and other safety conditions at time step \( k \) for constraint index \( j \). In deterministic planning, we require \( g_{k,j}(x_k, u_k) \le 0 \) for all \( k, j \). Under uncertainty, we instead impose chance constraints

\[ \mathbb{P}\big( g_{k,j}(x_k, u_k) \le 0 \big) \; \ge \; 1 - \alpha_{k,j}, \quad \forall k,j, \]

or, more compactly, a joint chance constraint

\[ \mathbb{P}\big( g_{k,j}(x_k, u_k) \le 0 \text{ for all } k,j \big) \; \ge \; 1 - \alpha. \]

A generic chance-constrained planning problem then reads

\[ \begin{aligned} \min_{\{u_k\}_{k=0}^{N-1}} \quad & \mathbb{E}\Big[ \sum_{k=0}^{N-1} \ell(x_k, u_k) + \ell_f(x_N) \Big] \\ \text{s.t.} \quad & x_{k+1} = f(x_k, u_k, w_k), \quad k = 0,\dots,N-1, \\ & \mathbb{P}\big( g_{k,j}(x_k, u_k) \le 0 \big) \ge 1 - \alpha_{k,j}, \quad \forall k,j, \\ & u_k \in \mathcal{U}, \quad k = 0,\dots,N-1. \end{aligned} \]

In full generality this problem is hard. The key simplification we exploit is that in linear–Gaussian settings, chance constraints involving linear functions of the state can be turned into deterministic convex inequalities.

Suppose at some time step \( k \) we know the belief over the state is Gaussian, \( x_k \sim \mathcal{N}(\mu_k, \Sigma_k) \), as obtained by propagation from Lesson 2. Consider a linear scalar constraint

\[ a^\top x_k \le b, \]

where \( a \in \mathbb{R}^n \) and \( b \in \mathbb{R} \) are known. We want

\[ \mathbb{P}(a^\top x_k \le b) \; \ge \; 1 - \varepsilon, \]

for some violation probability \( \varepsilon \in (0,1) \).

Distribution of the linear form. Define the scalar random variable

\[ y := a^\top x_k. \]

Since \( x_k \sim \mathcal{N}(\mu_k, \Sigma_k) \), we have

\[ y \sim \mathcal{N}\big( m_k, s_k^2 \big), \quad m_k = a^\top \mu_k, \quad s_k^2 = a^\top \Sigma_k a. \]

Thus

\[ \mathbb{P}(a^\top x_k \le b) = \mathbb{P}(y \le b) = \Phi\!\left(\frac{b - m_k}{s_k}\right), \]

where \( \Phi \) is the CDF of the standard normal distribution.

Equivalence to a deterministic inequality. The chance constraint \( \mathbb{P}(a^\top x_k \le b) \ge 1 - \varepsilon \) is equivalent to

\[ \Phi\!\left(\frac{b - m_k}{s_k}\right) \ge 1 - \varepsilon. \]

Since \( \Phi \) is strictly increasing, we obtain the equivalent condition

\[ \frac{b - m_k}{s_k} \; \ge \; \Phi^{-1}(1 - \varepsilon), \]

or, after rearranging,

\[ m_k + \beta_\varepsilon s_k \; \le \; b, \quad \beta_\varepsilon := \Phi^{-1}(1 - \varepsilon). \]

Substituting \( m_k = a^\top \mu_k \) and \( s_k = \sqrt{a^\top \Sigma_k a} \), we obtain the standard linear–Gaussian chance constraint formula

\[ a^\top \mu_k + \beta_\varepsilon \sqrt{a^\top \Sigma_k a} \; \le \; b. \]

Geometrically, this inflates the original constraint by a margin \( \beta_\varepsilon \sqrt{a^\top \Sigma_k a} \) measured in the direction \( a \). Smaller \( \varepsilon \) (more conservative planning) corresponds to larger \( \beta_\varepsilon \).

Convexity. For fixed covariance \( \Sigma_k \), the term \( \sqrt{a^\top \Sigma_k a} \) is a constant, so the constraint is linear in \( \mu_k \). When \( \mu_k \) depends linearly on the control sequence (as with linear dynamics), the feasible set in control space defined by this inequality is convex, and the overall problem becomes a convex program.

In motion planning we typically care about the probability of any violation along the entire trajectory. Let \( \mathcal{E}_{k,j} \) denote the event that constraint \( j \) at time \( k \) is violated, i.e. \( g_{k,j}(x_k, u_k) > 0 \). The joint chance constraint

\[ \mathbb{P}\big( g_{k,j}(x_k, u_k) \le 0 \;\; \forall k,j \big) \; \ge \; 1 - \alpha \]

is equivalent to

\[ \mathbb{P}\Big( \bigcup_{k,j} \mathcal{E}_{k,j} \Big) \; \le \; \alpha. \]

Exact enforcement of this joint probability is difficult. A widely used conservative approximation is based on the union bound (Boole's inequality):

\[ \mathbb{P}\Big( \bigcup_{k,j} \mathcal{E}_{k,j} \Big) \le \sum_{k,j} \mathbb{P}(\mathcal{E}_{k,j}). \]

Therefore, if we choose per-constraint risk allocations \( \alpha_{k,j} \) such that \( \sum_{k,j} \alpha_{k,j} \le \alpha \) and enforce

\[ \mathbb{P}\big( g_{k,j}(x_k, u_k) \le 0 \big) \ge 1 - \alpha_{k,j} \quad \forall k,j, \]

then automatically

\[ \mathbb{P}\Big( \bigcup_{k,j} \mathcal{E}_{k,j} \Big) \le \sum_{k,j} \mathbb{P}(\mathcal{E}_{k,j}) \le \sum_{k,j} \alpha_{k,j} \le \alpha, \]

so the joint chance constraint is satisfied. This is called risk allocation and yields a conservative but convex approximation of the path-wise probability constraint.

A simple strategy is uniform risk allocation: for \( K \) time steps and \( J \) constraints per time step, set \( \alpha_{k,j} = \alpha / (KJ) \). More sophisticated methods treat \( \alpha_{k,j} \) as decision variables and optimize them together with the trajectory.

flowchart TD
  A["Total risk alpha"] --> B["Choose per-step risks alpha_kj"]
  B --> C["For each (k,j): build single-step chance constraint"]
  C --> D["Solve convex program with all per-step constraints"]
  D --> E["Resulting plan has P(any violation) <= alpha (conservative)"]
        

Consider a 1D point-mass robot with position \( p_k \) and velocity \( v_k \). The state is \( x_k = [p_k \; v_k]^\top \in \mathbb{R}^2 \), and the control is acceleration \( u_k \in \mathbb{R} \). With sampling time \( \Delta t \), the standard discretized double integrator with additive Gaussian disturbance is

\[ x_{k+1} = A x_k + B u_k + w_k, \quad w_k \sim \mathcal{N}(0, W), \]

\[ A = \begin{bmatrix} 1 & \Delta t \\ 0 & 1 \end{bmatrix}, \quad B = \begin{bmatrix} \tfrac{1}{2}\Delta t^2 \\ \Delta t \end{bmatrix}. \]

Assume we know the initial belief \( x_0 \sim \mathcal{N}(\mu_0, \Sigma_0) \). Since the dynamics are linear and the noise is Gaussian, the belief remains Gaussian:

\[ \mu_{k+1} = A \mu_k + B u_k, \quad \Sigma_{k+1} = A \Sigma_k A^\top + W. \]

Note that \( \Sigma_k \) does not depend on the controls when the disturbance is additive with fixed covariance \( W \). This is crucial: it means the covariance sequence can be precomputed offline, independent of the chosen control policy.

Suppose the robot must stay inside a corridor \( 0 \le p_k \le p_{\max} \) with high probability. We focus on the upper wall, modeled as the linear constraint \( p_k \le p_{\max} \). Using \( a = [1 \; 0]^\top \) and \( b = p_{\max} \), the chance constraint

\[ \mathbb{P}(p_k \le p_{\max}) \ge 1 - \varepsilon_k \]

becomes, by the derivation in Section 3,

\[ a^\top \mu_k + \beta_{\varepsilon_k} \sqrt{a^\top \Sigma_k a} \le p_{\max}, \quad a = \begin{bmatrix} 1 \\ 0 \end{bmatrix}. \]

Since \( a^\top \Sigma_k a = (\Sigma_k)_{11} \) is just the variance of position at time \( k \), we write \( \sigma_{p,k}^2 = (\Sigma_k)_{11} \) to obtain

\[ \mu_{p,k} + \beta_{\varepsilon_k} \sigma_{p,k} \le p_{\max}, \]

where \( \mu_{p,k} \) is the first component of \( \mu_k \). This inequality is linear in the state mean \( \mu_k \), and thus linear in the controls \( u_0,\dots,u_{k-1} \).

Similar constraints can be written for lower corridor boundaries, obstacles modeled by supporting half-spaces of the configuration space, or velocity and acceleration limits.

We now implement a simple chance-constrained planner for the 1D double integrator using Python, numpy for linear algebra, and cvxpy for convex optimization. The goal is to drive the robot from an uncertain initial position to a target while keeping the position below \( p_{\max} \) with high probability at each step.

import numpy as np
import cvxpy as cp
from scipy.stats import norm

# Discrete-time double integrator parameters
dt = 0.1
A = np.array([[1.0, dt],
              [0.0, 1.0]])
B = np.array([[0.5 * dt**2],
              [dt]])

n = 2  # state dimension
m = 1  # control dimension
N = 30  # horizon length

# Process noise covariance and initial belief
W = np.diag([1e-4, 1e-4])
mu0 = np.array([0.0, 0.0])      # initial mean [p0, v0]
Sigma0 = np.diag([0.05**2, 0.02**2])

# Corridor upper bound and risk parameters
p_max = 2.0
alpha_total = 0.05      # total allowed path-wise risk
epsilon_k = alpha_total / N  # uniform per-step risk allocation
beta_eps = norm.ppf(1.0 - epsilon_k)

# Pre-compute covariance sequence (independent of controls)
Sigma_list = [Sigma0]
for k in range(N):
    Sigma_next = A @ Sigma_list[-1] @ A.T + W
    Sigma_list.append(Sigma_next)

sigma_p = [np.sqrt(Sigma_list[k][0, 0]) for k in range(N + 1)]

# Optimization variables
X = cp.Variable((n, N + 1))
U = cp.Variable((m, N))

# Objective: quadratic control effort + terminal position error
p_target = 1.5
cost = 0.0
constraints = []

# Initial condition on mean state
constraints.append(X[:, 0] == mu0)

for k in range(N):
    # Linear dynamics on mean
    constraints.append(X[:, k+1] == A @ X[:, k] + B @ U[:, k])

    # Chance constraint for position: P(p_k+1 <= p_max) >= 1 - epsilon_k
    # Using: mu_p + beta * sigma_p <= p_max  (sigma_p is precomputed constant)
    mu_p_k1 = X[0, k+1]
    constraints.append(mu_p_k1 + beta_eps * sigma_p[k+1] <= p_max)

    # Optional input bounds
    constraints.append(cp.abs(U[:, k]) <= 2.0)

    # Stage cost: penalize control effort and deviation from target position
    cost += 0.1 * cp.sum_squares(U[:, k])

# Terminal cost: encourage p_N close to p_target
cost += 10.0 * cp.sum_squares(X[0, N] - p_target)

prob = cp.Problem(cp.Minimize(cost), constraints)
prob.solve(solver=cp.OSQP)

print("Optimal cost:", prob.value)
print("Planned positions:", X.value[0, :])
print("Planned controls:", U.value[0, :])
      

The key modeling step is the conversion of the probabilistic constraints into the deterministic inequalities \( \mu_{p,k} + \beta_{\varepsilon_k} \sigma_{p,k} \le p_{\max} \), with \( \sigma_{p,k} \) precomputed from the covariance recursion. In higher-dimensional systems, similar patterns apply with different \( a \) vectors for each half-space of the configuration-space obstacles.

In C++, a common robotics stack uses Eigen for linear algebra, plus a trajectory optimization or model-predictive-control library. Below we sketch a helper function that evaluates a single linear–Gaussian chance constraint \( \mathbb{P}(a^\top x \le b) \ge 1 - \varepsilon \) using the analytic formula, and a simple loop that checks a planned trajectory.

#include <iostream>
#include <cmath>
#include <vector>
#include <Eigen/Dense>

double normalQuantile(double p) {
    // In practice use boost::math or another reliable implementation.
    // Here we assume access to std::erfcinv (C++17 extension or vendor-specific).
    // Phi^{-1}(p) = -sqrt(2) * erfcinv(2p)
    return -std::sqrt(2.0) * std::erfcinv(2.0 * p);
}

bool chanceConstraintSatisfied(
    const Eigen::VectorXd& mu,
    const Eigen::MatrixXd& Sigma,
    const Eigen::VectorXd& a,
    double b,
    double epsilon
) {
    double m = a.transpose() * mu;
    double s2 = (a.transpose() * Sigma * a)(0, 0);
    if (s2 < 0.0) {
        std::cerr << "Warning: negative variance, clipping to zero\n";
        s2 = 0.0;
    }
    double s = std::sqrt(s2);
    double beta = normalQuantile(1.0 - epsilon);
    // Deterministic equivalent: m + beta * s <= b
    return (m + beta * s) <= b + 1e-9;
}

int main() {
    // Example: 2D state [p; v], double integrator covariance at one time step
    Eigen::Vector2d mu;
    mu << 1.0, 0.2;
    Eigen::Matrix2d Sigma;
    Sigma << 0.05 * 0.05, 0.0,
             0.0,         0.02 * 0.02;

    Eigen::Vector2d a;
    a << 1.0, 0.0; // constrain position only
    double p_max = 2.0;
    double epsilon = 0.01;

    bool ok = chanceConstraintSatisfied(mu, Sigma, a, p_max, epsilon);
    std::cout << "Chance constraint satisfied? " << (ok ? "yes" : "no") << std::endl;

    return 0;
}
      

In a full planner (e.g. using OMPL for continuous C-space planning), one can use this function to filter edges or states that violate chance constraints, possibly in combination with an underlying deterministic planner that proposes candidate paths.

In Java, linear algebra and probability utilities can be taken from org.apache.commons.math3. The following code illustrates how to implement a single linear–Gaussian chance constraint evaluation.

import org.apache.commons.math3.linear.RealMatrix;
import org.apache.commons.math3.linear.RealVector;
import org.apache.commons.math3.linear.MatrixUtils;
import org.apache.commons.math3.distribution.NormalDistribution;

public class ChanceConstraint {

    private static final NormalDistribution stdNormal =
        new NormalDistribution(0.0, 1.0);

    public static boolean isSatisfied(
            RealVector mu,
            RealMatrix Sigma,
            RealVector a,
            double b,
            double epsilon
    ) {
        double m = a.dotProduct(mu);
        RealVector Sigma_a = Sigma.operate(a);
        double s2 = a.dotProduct(Sigma_a);
        if (s2 < 0.0) {
            s2 = 0.0;
        }
        double s = Math.sqrt(s2);
        double beta = stdNormal.inverseCumulativeProbability(1.0 - epsilon);
        return m + beta * s <= b + 1e-9;
    }

    public static void main(String[] args) {
        // Example: 2D state
        double[][] SigmaData = {
            {0.05 * 0.05, 0.0},
            {0.0, 0.02 * 0.02}
        };
        RealMatrix Sigma = MatrixUtils.createRealMatrix(SigmaData);
        RealVector mu = MatrixUtils.createRealVector(new double[]{1.0, 0.2});
        RealVector a = MatrixUtils.createRealVector(new double[]{1.0, 0.0});
        double pMax = 2.0;
        double epsilon = 0.01;

        boolean ok = isSatisfied(mu, Sigma, a, pMax, epsilon);
        System.out.println("Chance constraint satisfied? " + ok);
    }
}
      

The same routine can be embedded in a Java-based trajectory optimizer or MPC controller, where the mean \( \mu_k \) is propagated using linear dynamics and the covariance is precomputed from the noise model.

MATLAB is well-suited for prototyping chance-constrained planners, and Simulink can be used to simulate the stochastic closed-loop system. Below is a MATLAB script that:

1. Propagates the mean and covariance for the double integrator.
2. Builds deterministic inequality margins for a per-step chance constraint.
3. Solves a quadratic program using quadprog.

dt = 0.1;
A = [1, dt;
     0, 1];
B = [0.5 * dt^2;
     dt];

n = 2; m = 1; N = 30;
W = diag([1e-4, 1e-4]);

mu0 = [0; 0];
Sigma0 = diag([0.05^2, 0.02^2]);

p_max = 2.0;
alpha_total = 0.05;
epsilon_k = alpha_total / N;
beta_eps = norminv(1 - epsilon_k, 0, 1);

% Precompute covariance sequence
Sigma = cell(N+1, 1);
Sigma{1} = Sigma0;
sigma_p = zeros(N+1, 1);
sigma_p(1) = sqrt(Sigma0(1, 1));
for k = 1:N
    Sigma{k+1} = A * Sigma{k} * A' + W;
    sigma_p(k+1) = sqrt(Sigma{k+1}(1, 1));
end

% Build condensed dynamics: X = Sx * x0 + Su * U
% Here we only need the mean position, so we could build linear maps for p_k.
% For clarity, we keep state dimension n=2.
Sx = zeros(n*(N+1), n);
Sx(1:n, :) = eye(n);
for k = 1:N
    Sx(k*n+1:(k+1)*n, :) = A^k;
end

Su = zeros(n*(N+1), m*N);
for k = 1:N
    for j = 1:k
        row_idx = k*n+1:(k+1)*n;
        col_idx = (j-1)*m+1:j*m;
        Su(row_idx, col_idx) = A^(k-j) * B;
    end
end

% Quadratic cost: minimize sum(u_k^2) + terminal position error
Q_u = 0.1 * eye(m*N);
Q_term = 10.0;
p_target = 1.5;

% Terminal position selector
E_pN = zeros(1, n*(N+1));
E_pN( (N)*n + 1 ) = 1;  % picks p_N

H = 2 * (Q_u + Q_term * (Su' * (E_pN' * E_pN) * Su));
f = 2 * Q_term * Su' * (E_pN' * (E_pN * Sx * mu0 - p_target));

% Chance constraints: mu_p_k + beta * sigma_p(k) <= p_max
Aineq = [];
bineq = [];

for k = 1:N
    E_pk = zeros(1, n*(N+1));
    E_pk(k*n + 1) = 1; % extract p_k
    row = E_pk * Su;
    offset = E_pk * Sx * mu0 + beta_eps * sigma_p(k+1);
    Aineq = [Aineq; row];
    bineq = [bineq; p_max - offset];
end

% Optional input bounds: -2 <= u_k <= 2
Aineq = [Aineq; eye(m*N); -eye(m*N)];
bineq = [bineq; 2*ones(m*N, 1); 2*ones(m*N, 1)];

options = optimoptions('quadprog','Display','off');
[U_opt, fval, exitflag] = quadprog(H, f, Aineq, bineq, [], [], [], [], [], options);

disp(['Exitflag: ', num2str(exitflag)]);
      

In Simulink, one can represent the same stochastic dynamics by:

• A state-space block implementing the deterministic part \( x_{k+1} = A x_k + B u_k \).
• A noise block injecting Gaussian disturbances into the state.
• A supervisor block that computes the deterministic chance-constraint margins and saturates or adjusts the control signal \( u_k \) to remain within the risk budget.

This enables Monte Carlo validation of the theoretical chance constraints derived earlier.

Wolfram Mathematica provides symbolic and numeric tools that can be used to experiment with chance-constrained planning. Below we show a small example that solves a single-step chance-constrained problem for a scalar Gaussian state.

(* Parameters *)
mu = 1.0;
sigma = 0.2;
pMax = 2.0;
epsilon = 0.01;

(* Beta from inverse CDF of standard normal *)
beta = InverseCDF[NormalDistribution[0, 1], 1 - epsilon];

(* Decision variable: shift the mean by control u *)
u = Symbol["u", Reals];

(* Chance constraint: mu + u + beta * sigma <= pMax *)
chanceConstraint = mu + u + beta * sigma <= pMax;

(* Cost: quadratic control effort *)
cost = u^2;

solution = NMinimize[
  {cost, chanceConstraint},
  u
];

solution
      

For multi-dimensional linear systems, one can build the mean and covariance recursions using MatrixExp or explicit discrete-time matrices and then derive the deterministic inequalities symbolically or numerically before using NMinimize or QuadraticOptimization.

Problem 1 (Single-Step Linear-Gaussian Chance Constraint): Let \( x \sim \mathcal{N}(\mu, \Sigma) \) and consider the scalar constraint \( a^\top x \le b \). Show that the chance constraint \( \mathbb{P}(a^\top x \le b) \ge 1 - \varepsilon \) is equivalent to \( a^\top \mu + \beta_\varepsilon \sqrt{a^\top \Sigma a} \le b \) with \( \beta_\varepsilon = \Phi^{-1}(1 - \varepsilon) \).

Solution: Define \( y = a^\top x \). Since \( x \) is Gaussian, \( y \) is Gaussian with mean \( m = a^\top \mu \) and variance \( s^2 = a^\top \Sigma a \). Thus

\[ \mathbb{P}(a^\top x \le b) = \mathbb{P}(y \le b) = \Phi\!\left(\frac{b - m}{s}\right), \]

where \( \Phi \) is the standard normal CDF. The chance constraint \( \mathbb{P}(a^\top x \le b) \ge 1 - \varepsilon \) is therefore

\[ \Phi\!\left(\frac{b - m}{s}\right) \ge 1 - \varepsilon. \]

Since \( \Phi \) is strictly increasing, applying \( \Phi^{-1} \) to both sides yields

\[ \frac{b - m}{s} \ge \Phi^{-1}(1 - \varepsilon) = \beta_\varepsilon. \]

Rearranging gives

\[ m + \beta_\varepsilon s \le b \quad \Longleftrightarrow \quad a^\top \mu + \beta_\varepsilon \sqrt{a^\top \Sigma a} \le b, \]

as required.

Problem 2 (Union Bound and Risk Allocation): Let \( \{\mathcal{E}_i\}_{i=1}^M \) be a collection of violation events (e.g., constraints at different time steps). Suppose we choose risk allocations \( \alpha_i \) such that \( \mathbb{P}(\mathcal{E}_i) \le \alpha_i \) and \( \sum_{i=1}^M \alpha_i \le \alpha \). Show that \( \mathbb{P}(\bigcup_{i=1}^M \mathcal{E}_i) \le \alpha \).

Solution: By Boole's inequality (union bound),

\[ \mathbb{P}\Big(\bigcup_{i=1}^M \mathcal{E}_i\Big) \le \sum_{i=1}^M \mathbb{P}(\mathcal{E}_i). \]

Using the assumption \( \mathbb{P}(\mathcal{E}_i) \le \alpha_i \), we obtain

\[ \mathbb{P}\Big(\bigcup_{i=1}^M \mathcal{E}_i\Big) \le \sum_{i=1}^M \alpha_i \le \alpha. \]

Therefore the joint violation probability is bounded by \( \alpha \), as desired.

Problem 3 (Convexity of Linear-Gaussian Chance Constraints): Consider linear dynamics and additive Gaussian noise, so that \( x_k \) is an affine function of the control sequence \( u_0,\dots,u_{N-1} \). Use the formula from Problem 1 to argue that the feasible set of controls defined by \( \mathbb{P}(a_k^\top x_k \le b_k) \ge 1 - \varepsilon_k \) is convex.

Solution: For each \( k \), we have \( x_k = M_k u + c_k \), where \( u \) stacks all controls and \( M_k \), \( c_k \) are fixed matrices/vectors (due to linear dynamics). The corresponding chance constraint is equivalent to

\[ a_k^\top \mu_k + \beta_{\varepsilon_k} \sqrt{a_k^\top \Sigma_k a_k} \le b_k. \]

The covariance \( \Sigma_k \) depends only on the noise and dynamics, not on \( u \), so \( \sqrt{a_k^\top \Sigma_k a_k} \) is constant. Meanwhile, \( \mu_k = M_k u + c_k \), so \( a_k^\top \mu_k = a_k^\top M_k u + a_k^\top c_k \) is an affine function of \( u \). Therefore each constraint has the form

\[ \underbrace{(a_k^\top M_k) u + a_k^\top c_k + \beta_{\varepsilon_k} \sqrt{a_k^\top \Sigma_k a_k}}_{\text{affine in } u} \le b_k, \]

which defines a half-space in control space and is convex. The intersection over all \( k \) of such half-spaces is also convex, so the overall feasible set is convex.

Problem 4 (Mean and Covariance Recursion): For the linear system \( x_{k+1} = A x_k + B u_k + w_k \) with \( w_k \sim \mathcal{N}(0, W) \) i.i.d., show that if \( x_0 \sim \mathcal{N}(\mu_0, \Sigma_0) \), then \( x_k \sim \mathcal{N}(\mu_k, \Sigma_k) \) with

\[ \mu_{k+1} = A \mu_k + B u_k, \quad \Sigma_{k+1} = A \Sigma_k A^\top + W. \]

Solution: Since \( x_k \) is Gaussian and \( w_k \) is Gaussian, and they are independent, their affine combination is Gaussian. Taking expectations,

\[ \mu_{k+1} = \mathbb{E}[x_{k+1}] = \mathbb{E}[A x_k + B u_k + w_k] = A \mathbb{E}[x_k] + B u_k + \mathbb{E}[w_k] = A \mu_k + B u_k, \]

because \( \mathbb{E}[w_k] = 0 \). For the covariance,

\[ \Sigma_{k+1} = \mathbb{E}\big[(x_{k+1} - \mu_{k+1})(x_{k+1} - \mu_{k+1})^\top\big]. \]

Substitute \( x_{k+1} = A x_k + B u_k + w_k \) and use \( \mu_{k+1} = A \mu_k + B u_k \) to obtain \( x_{k+1} - \mu_{k+1} = A(x_k - \mu_k) + w_k \). Thus

\[ \Sigma_{k+1} = A \Sigma_k A^\top + \mathbb{E}[w_k w_k^\top] + A \mathbb{E}[(x_k - \mu_k) w_k^\top] + \mathbb{E}[w_k (x_k - \mu_k)^\top] A^\top. \]

Independence implies \( \mathbb{E}[(x_k - \mu_k) w_k^\top] = 0 \), and \( \mathbb{E}[w_k w_k^\top] = W \), so we obtain \( \Sigma_{k+1} = A \Sigma_k A^\top + W \).

Problem 5 (Uniform Risk Allocation): Suppose we have an horizon of \( N \) time steps and wish to keep the probability of any constraint violation below \( \alpha \). Using uniform risk allocation, choose \( \alpha_k = \alpha / N \) and per-step chance constraints \( \mathbb{P}(\mathcal{E}_k) \le \alpha_k \). Give an upper bound on the total violation probability and explain why this approach is conservative.

Solution: By the union bound,

\[ \mathbb{P}\Big(\bigcup_{k=1}^N \mathcal{E}_k\Big) \le \sum_{k=1}^N \mathbb{P}(\mathcal{E}_k) \le \sum_{k=1}^N \alpha_k = \sum_{k=1}^N \frac{\alpha}{N} = \alpha. \]

The bound can be strict when the events are not disjoint. If different violations are correlated or overlapping, the true probability of at least one violation can be significantly smaller than \( \alpha \). Thus uniform risk allocation is conservative: it may force the planner to be more cautious than necessary to meet the desired risk level.

In this lesson we introduced chance-constrained planning as a principled way to incorporate uncertainty into motion planning. Starting from a belief-space representation, we formalized probabilistic safety requirements and showed how, in linear–Gaussian systems, they reduce to deterministic convex inequalities on the state mean and covariance. We derived the standard linear–Gaussian chance constraint formula, discussed path-wise risk allocation using the union bound, and demonstrated implementations in Python, C++, Java, MATLAB/Simulink, and Wolfram Mathematica.

These tools enable risk-bounded trajectory optimization and serve as a bridge toward more general planning under uncertainty frameworks, such as POMDP-based methods, which will be explored in the next lesson.