In classical motion planning (previous chapters), we assume the robot state \( x_t \) is perfectly known, and we plan trajectories \( x_0, x_1, \dots, x_T \) in configuration or state space. Under uncertainty, the robot only has noisy sensors and imperfect actuators. Instead of a point \( x_t \), it maintains a belief:

\[ b_t(x) = p(x_t = x \mid z_{0:t}, u_{0:t-1}) , \]

where \( z_{0:t} \) is the observation history and \( u_{0:t-1} \) the control history. The planner then chooses controls as functions of beliefs: \( u_t = \pi(b_t) \), not of unknown true states.

Intuitively, belief-space planning answers: what should the robot do, given what it currently knows (and does not know) about its state? This is crucial in tasks like navigation in fog, manipulation with occlusions, or any situation with partial observability.

flowchart TD
  H["History (past z, u)"] --> B["Belief b_t (distribution over state)"]
  B --> P["Planner on beliefs"]
  P --> U["Control u_t"]
  U --> W["Robot & world"]
  W --> Z["New observation z_{t+1}"]
  Z --> H2["Updated history"]
  H2 --> B2["New belief b_{t+1}"]
        

The core idea: instead of planning in the physical state space \( \mathcal{X} \), we plan in the belief space \( \mathcal{B} \), the set of all probability distributions over \( \mathcal{X} \). This space is usually infinite-dimensional, but it becomes finite-dimensional under parametric approximations such as Gaussians.

Consider a stochastic dynamical system with state \( x_t \in \mathcal{X} \subset \mathbb{R}^n \), control \( u_t \in \mathcal{U} \), and observation \( z_t \in \mathcal{Z} \), with models

\[ p(x_{t+1} \mid x_t, u_t), \qquad p(z_t \mid x_t). \]

The belief update from time \( t \) to \( t+1 \) decomposes into a prediction and an update (Bayes filter):

\[ \bar{b}_{t+1}(x') = \int p(x' \mid x, u_t)\, b_t(x)\, dx \]

\[ b_{t+1}(x') = \eta \, p(z_{t+1} \mid x') \, \bar{b}_{t+1}(x'), \]

where \( \eta \) is a normalizing constant ensuring \( \int b_{t+1}(x')\, dx' = 1 \). For discrete state spaces, integrals become sums. The prediction involves only the motion model and control, while the update incorporates the new observation.

We can view this update as a (random) map \( \tau \) on beliefs:

\[ b_{t+1} = \tau(b_t, u_t, z_{t+1}). \]

For many robotic systems, exact beliefs are intractable. Common belief representations include:

• Gaussian belief: parameterized by mean and covariance \( (\boldsymbol{\mu}_t, \mathbf{\Sigma}_t) \).
• Gaussian mixtures: finite mixture of Gaussians, capturing multi-modality.
• Particle beliefs: weighted samples \( \{(x_t^{(i)}, w_t^{(i)})\}_{i=1}^N \).

In this lesson we focus on the linear-Gaussian case, where the Bayes filter reduces to a Kalman filter and the belief space becomes the space of Gaussian parameters.

Although the underlying robot state is only partially observed, the belief \( b_t \) is a sufficient statistic of the entire history. This means the belief process is Markovian.

Proposition (Belief Markov Property). For any admissible policy,

\[ p(b_{t+1} \mid b_0, u_0, \dots, b_t, u_t) = p(b_{t+1} \mid b_t, u_t). \]

Sketch of proof.

1. By definition, \( b_t \) encodes \( p(x_t \mid z_{0:t}, u_{0:t-1}) \).
2. The probability of the next state and observation satisfies

   \[ p(x_{t+1}, z_{t+1} \mid z_{0:t}, u_{0:t}) = \int p(x_{t+1} \mid x_t, u_t)\, p(z_{t+1} \mid x_{t+1})\, p(x_t \mid z_{0:t}, u_{0:t-1})\, dx_t. \]

3. The integral depends on history only through \( p(x_t \mid z_{0:t}, u_{0:t-1}) = b_t \), so the distribution of \( (x_{t+1}, z_{t+1}) \) given history depends only on \( (b_t, u_t) \).
4. The next belief \( b_{t+1} \) is a deterministic transformation of \( (b_t, u_t, z_{t+1}) \) via the Bayes filter, so its distribution conditioned on the past is fully determined by \( (b_t, u_t) \).

Therefore, the belief process is a controlled Markov process with state space \( \mathcal{B} \). We can define a belief-state MDP with:

• state: belief \( b \in \mathcal{B} \),
• control: \( u \in \mathcal{U} \),
• transition: distribution over next beliefs induced by \( \tau(b, u, z') \) and \( p(z' \mid b, u) \),
• stage cost: \( c_B(b, u) = \mathbb{E}_{x \sim b}[c(x, u)] \).

For finite horizons, the optimal value function \( V_t^*(b) \) obeys the Bellman equation

\[ V_t^*(b) = \min_{u \in \mathcal{U}} \left\{ c_B(b, u) + \gamma \int V_{t+1}^*\big(\tau(b, u, z')\big) \, p(z' \mid b, u)\, dz' \right\}, \]

with terminal cost \( V_T^*(b) \). This is the mathematical core of belief-space planning; when the underlying problem is formulated formally, it matches the well-known partially observable Markov decision process framework (to be studied later in Chapter 6).

A particularly important special case for robot motion is the linear-Gaussian system:

\[ x_{t+1} = \mathbf{A} x_t + \mathbf{B} u_t + w_t, \quad w_t \sim \mathcal{N}(\mathbf{0}, \mathbf{Q}), \]

\[ z_t = \mathbf{C} x_t + v_t, \quad v_t \sim \mathcal{N}(\mathbf{0}, \mathbf{R}), \]

with Gaussian prior \( x_0 \sim \mathcal{N}(\boldsymbol{\mu}_0, \mathbf{\Sigma}_0) \). It is a classical result that the belief remains Gaussian at all times: \( b_t = \mathcal{N}(\boldsymbol{\mu}_t, \mathbf{\Sigma}_t) \).

The Bayes filter becomes the Kalman filter. Using the prediction-update notation,

\[ \text{Prediction:}\quad \boldsymbol{\mu}_{t+1}^- = \mathbf{A}\boldsymbol{\mu}_t + \mathbf{B} u_t, \quad \mathbf{\Sigma}_{t+1}^- = \mathbf{A}\mathbf{\Sigma}_t\mathbf{A}^\top + \mathbf{Q}, \]

\[ \text{Update:}\quad \mathbf{K}_{t+1} = \mathbf{\Sigma}_{t+1}^- \mathbf{C}^\top \big(\mathbf{C}\mathbf{\Sigma}_{t+1}^- \mathbf{C}^\top + \mathbf{R}\big)^{-1}, \]

\[ \boldsymbol{\mu}_{t+1} = \boldsymbol{\mu}_{t+1}^- + \mathbf{K}_{t+1}\big(z_{t+1} - \mathbf{C}\boldsymbol{\mu}_{t+1}^-\big), \]

\[ \mathbf{\Sigma}_{t+1} = (\mathbf{I} - \mathbf{K}_{t+1}\mathbf{C})\, \mathbf{\Sigma}_{t+1}^-. \]

In belief-space planning, the belief state is then the pair \( (\boldsymbol{\mu}_t, \mathbf{\Sigma}_t) \). The dynamics of \( (\boldsymbol{\mu}_t, \mathbf{\Sigma}_t) \) are deterministic given \( (u_t, z_{t+1}) \); stochasticity enters through the random observation \( z_{t+1} \).

This representation is frequently used in robotics to design planners that trade off reaching a goal configuration and maintaining low uncertainty (e.g., staying near informative sensor viewpoints).

In belief-space planning we design stage costs that depend on both the expected state and its uncertainty. A common choice in continuous spaces is quadratic cost in deviation from goal plus a penalty on covariance:

\[ c_B(b_t, u_t) = \mathbb{E}_{x \sim b_t} \big[(x - x_{\text{goal}})^\top \mathbf{Q}_c (x - x_{\text{goal}})\big] + \lambda \,\mathrm{tr}(\mathbf{\Sigma}_t) + u_t^\top \mathbf{R}_c u_t. \]

For Gaussian belief \( b_t = \mathcal{N}(\boldsymbol{\mu}_t, \mathbf{\Sigma}_t) \), the expectation can be computed analytically.

Proposition. Let \( x \sim \mathcal{N}(\boldsymbol{\mu}, \mathbf{\Sigma}) \). Then

\[ \mathbb{E}\big[(x - x_{\text{goal}})^\top \mathbf{Q}_c (x - x_{\text{goal}})\big] = (\boldsymbol{\mu} - x_{\text{goal}})^\top \mathbf{Q}_c (\boldsymbol{\mu} - x_{\text{goal}}) + \mathrm{tr}(\mathbf{Q}_c \mathbf{\Sigma}). \]

Proof. Write \( x - x_{\text{goal}} = (\boldsymbol{\mu} - x_{\text{goal}}) + \tilde{x} \) where \( \tilde{x} = x - \boldsymbol{\mu} \) has zero mean and covariance \( \mathbf{\Sigma} \). Expanding the quadratic form,

\[ (x - x_{\text{goal}})^\top \mathbf{Q}_c (x - x_{\text{goal}}) = (\boldsymbol{\mu} - x_{\text{goal}})^\top \mathbf{Q}_c (\boldsymbol{\mu} - x_{\text{goal}}) + 2 (\boldsymbol{\mu} - x_{\text{goal}})^\top \mathbf{Q}_c \tilde{x} + \tilde{x}^\top \mathbf{Q}_c \tilde{x}. \]

Taking expectations, the cross term vanishes because \( \mathbb{E}[\tilde{x}] = 0 \), and \( \mathbb{E}[\tilde{x}^\top \mathbf{Q}_c \tilde{x}] = \mathrm{tr}(\mathbf{Q}_c \mathbf{\Sigma}) \). This yields the claimed expression.

Thus, minimizing expected squared distance to the goal under Gaussian belief is equivalent to penalizing the mean error plus the covariance weighted by \( \mathbf{Q}_c \). The additional term \( \lambda \,\mathrm{tr}(\mathbf{\Sigma}_t) \) lets us directly penalize uncertainty independent of the goal.

Consider a 1D robot on a line with position \( x_t \in \mathbb{R} \) and control \( u_t \in \mathbb{R} \) (commanded displacement). Dynamics and sensing are

\[ x_{t+1} = x_t + u_t + w_t, \quad w_t \sim \mathcal{N}(0, \sigma_w^2), \]

\[ z_t = x_t + v_t, \quad v_t \sim \mathcal{N}(0, \sigma_v^2). \]

Assume Gaussian belief \( x_t \sim \mathcal{N}(\mu_t, \sigma_t^2) \). The scalar Kalman filter gives

\[ \mu_{t+1}^- = \mu_t + u_t, \quad (\sigma_{t+1}^-)^2 = \sigma_t^2 + \sigma_w^2, \]

\[ K_{t+1} = \frac{(\sigma_{t+1}^-)^2}{(\sigma_{t+1}^-)^2 + \sigma_v^2}, \quad \mu_{t+1} = \mu_{t+1}^- + K_{t+1}(z_{t+1} - \mu_{t+1}^-), \]

\[ \sigma_{t+1}^2 = (1 - K_{t+1})\, (\sigma_{t+1}^-)^2. \]

Suppose the robot can choose between two actions: \( u_t \in \{-u_{\max}, +u_{\max}\} \), with a goal position \( x_{\text{goal}} \). If we approximate one-step belief-space planning by ignoring the next measurement (i.e., using the predicted belief), the approximate one-step cost is

\[ c_B(b_t, u_t) \approx (\mu_t + u_t - x_{\text{goal}})^2 + \lambda \big(\sigma_t^2 + \sigma_w^2\big). \]

The planner chooses \( u_t \) minimizing this cost. Note that both actions have the same predicted variance; the decision is driven by the predicted mean offset from the goal. In higher dimensions, control can also affect the rate of future information gain (through the observation model), which leads to more interesting exploration-exploitation trade-offs.

flowchart TD
  B["Belief parameters (mu_t, Sigma_t)"] --> EVAL["Evaluate candidate controls in belief space"]
  EVAL --> SEL["Select control with minimal expected cost"]
  SEL --> EXEC["Execute control on robot"]
  EXEC --> OBS["Measure new observation"]
  OBS --> UPD["Update belief via Bayes/Kalman filter"]
  UPD --> B
        

We implement a small linear-Gaussian belief update and one-step belief-space planner in Python. In realistic systems, such code would be embedded inside a robotics stack using, for example, roboticstoolbox-python for kinematics and ROS/MoveIt/OMPL for motion planning. Here we focus on the belief component.

import numpy as np

class GaussianBelief1D:
    def __init__(self, mu, sigma2):
        self.mu = float(mu)
        self.sigma2 = float(sigma2)

    def predict(self, u, sigma_w2):
        """
        Linear 1D dynamics: x_{t+1} = x_t + u + w_t
        """
        mu_pred = self.mu + float(u)
        sigma2_pred = self.sigma2 + float(sigma_w2)
        return GaussianBelief1D(mu_pred, sigma2_pred)

    def update(self, z, sigma_v2):
        """
        1D Kalman update using measurement z = x + v.
        """
        sigma2_pred = self.sigma2
        K = sigma2_pred / (sigma2_pred + float(sigma_v2))
        mu_post = self.mu + K * (float(z) - self.mu)
        sigma2_post = (1.0 - K) * sigma2_pred
        return GaussianBelief1D(mu_post, sigma2_post)

def one_step_cost(belief, u, x_goal, sigma_w2, lam):
    """
    Approximate one-step belief-space cost using predicted belief,
    ignoring the update step (open-loop approximation).
    """
    pred = belief.predict(u, sigma_w2)
    mu_err = pred.mu - float(x_goal)
    return mu_err * mu_err + lam * pred.sigma2

def choose_control(belief, actions, x_goal, sigma_w2, lam):
    best_u = None
    best_cost = float("inf")
    for u in actions:
        cost = one_step_cost(belief, u, x_goal, sigma_w2, lam)
        if cost < best_cost:
            best_cost = cost
            best_u = u
    return best_u, best_cost

if __name__ == "__main__":
    # Current belief: x ~ N(mu, sigma2)
    belief = GaussianBelief1D(mu=0.0, sigma2=1.0)
    x_goal = 5.0
    sigma_w2 = 0.1
    lam = 0.5
    actions = [-1.0, 0.0, 1.0]

    u_star, J_star = choose_control(belief, actions, x_goal, sigma_w2, lam)
    print("Chosen control:", u_star)
    print("Approximate one-step cost:", J_star)
      

The structure of this code extends straightforwardly to multivariate Gaussians using numpy matrices. For higher-dimensional manipulators, beliefs are often maintained in the joint configuration space and coupled to forward kinematics from a robotics toolbox.

In C++, belief-space planners are frequently integrated with ROS, OMPL, and MoveIt. Below is a minimal 1D Gaussian belief class using Eigen. It can be generalized to vector states by replacing scalars with Eigen::VectorXd and Eigen::MatrixXd.

#include <iostream>
#include <cmath>

struct GaussianBelief1D {
    double mu;
    double sigma2;

    GaussianBelief1D(double mu_in = 0.0, double sigma2_in = 1.0)
        : mu(mu_in), sigma2(sigma2_in) {}

    GaussianBelief1D predict(double u, double sigma_w2) const {
        double mu_pred = mu + u;
        double sigma2_pred = sigma2 + sigma_w2;
        return GaussianBelief1D(mu_pred, sigma2_pred);
    }

    GaussianBelief1D update(double z, double sigma_v2) const {
        double K = sigma2 / (sigma2 + sigma_v2);
        double mu_post = mu + K * (z - mu);
        double sigma2_post = (1.0 - K) * sigma2;
        return GaussianBelief1D(mu_post, sigma2_post);
    }
};

double one_step_cost(const GaussianBelief1D& b,
                     double u,
                     double x_goal,
                     double sigma_w2,
                     double lambda_unc) {
    GaussianBelief1D pred = b.predict(u, sigma_w2);
    double mu_err = pred.mu - x_goal;
    return mu_err * mu_err + lambda_unc * pred.sigma2;
}

double choose_control(const GaussianBelief1D& b,
                      const double* actions,
                      std::size_t n_actions,
                      double x_goal,
                      double sigma_w2,
                      double lambda_unc) {
    double best_u = actions[0];
    double best_cost = one_step_cost(b, best_u, x_goal, sigma_w2, lambda_unc);
    for (std::size_t i = 1; i < n_actions; ++i) {
        double u = actions[i];
        double cost = one_step_cost(b, u, x_goal, sigma_w2, lambda_unc);
        if (cost < best_cost) {
            best_cost = cost;
            best_u = u;
        }
    }
    return best_u;
}

int main() {
    GaussianBelief1D b(0.0, 1.0);
    double actions[3] = {-1.0, 0.0, 1.0};
    double x_goal = 5.0;
    double sigma_w2 = 0.1;
    double lambda_unc = 0.5;

    double u_star = choose_control(b, actions, 3, x_goal, sigma_w2, lambda_unc);
    std::cout << "Chosen control: " << u_star << std::endl;
    return 0;
}
      

In a full ROS/OMPL setup, the belief would be attached to an OMPL state (e.g., joint configuration). The planner would sample trajectories in belief space or augment the cost function in state-space sampling-based planners with belief-dependent terms such as predicted covariance.

Java is less common for low-level robotics, but it appears in high-level decision-making and simulation (e.g., some planning libraries and educational frameworks). We give a simple 1D Gaussian belief class without external linear algebra dependencies. For higher dimensions, one may use libraries such as EJML or Apache Commons Math.

public class GaussianBelief1D {
    private double mu;
    private double sigma2;

    public GaussianBelief1D(double mu, double sigma2) {
        this.mu = mu;
        this.sigma2 = sigma2;
    }

    public double getMu() {
        return mu;
    }

    public double getSigma2() {
        return sigma2;
    }

    public GaussianBelief1D predict(double u, double sigmaW2) {
        double muPred = mu + u;
        double sigma2Pred = sigma2 + sigmaW2;
        return new GaussianBelief1D(muPred, sigma2Pred);
    }

    public GaussianBelief1D update(double z, double sigmaV2) {
        double K = sigma2 / (sigma2 + sigmaV2);
        double muPost = mu + K * (z - mu);
        double sigma2Post = (1.0 - K) * sigma2;
        return new GaussianBelief1D(muPost, sigma2Post);
    }

    public static double oneStepCost(GaussianBelief1D b,
                                     double u,
                                     double xGoal,
                                     double sigmaW2,
                                     double lambdaUnc) {
        GaussianBelief1D pred = b.predict(u, sigmaW2);
        double muErr = pred.getMu() - xGoal;
        return muErr * muErr + lambdaUnc * pred.getSigma2();
    }

    public static double chooseControl(GaussianBelief1D b,
                                       double[] actions,
                                       double xGoal,
                                       double sigmaW2,
                                       double lambdaUnc) {
        double bestU = actions[0];
        double bestCost = oneStepCost(b, bestU, xGoal, sigmaW2, lambdaUnc);
        for (int i = 1; i < actions.length; ++i) {
            double u = actions[i];
            double cost = oneStepCost(b, u, xGoal, sigmaW2, lambdaUnc);
            if (cost < bestCost) {
                bestCost = cost;
                bestU = u;
            }
        }
        return bestU;
    }

    public static void main(String[] args) {
        GaussianBelief1D belief = new GaussianBelief1D(0.0, 1.0);
        double[] actions = {-1.0, 0.0, 1.0};
        double xGoal = 5.0;
        double sigmaW2 = 0.1;
        double lambdaUnc = 0.5;

        double uStar = chooseControl(belief, actions, xGoal, sigmaW2, lambdaUnc);
        System.out.println("Chosen control: " + uStar);
    }
}
      

This structure can be embedded in higher-level Java planning frameworks where the robot dynamics and observation models are wrapped by Java interfaces and the belief update acts as a filter object.

MATLAB and Simulink are widely used for rapid prototyping of estimation and control algorithms. Below is a function implementing 1D Gaussian prediction and update. In Simulink, one would typically realize the same equations using blocks (sum, gain, and a Kalman Filter block from the Control System Toolbox).

function [mu_next, sigma2_next] = belief_step_1d(mu, sigma2, u, z, sigma_w2, sigma_v2)
% One-step 1D Gaussian belief update for linear sensor.
% Prediction
mu_pred = mu + u;
sigma2_pred = sigma2 + sigma_w2;

% Update
K = sigma2_pred / (sigma2_pred + sigma_v2);
mu_next = mu_pred + K * (z - mu_pred);
sigma2_next = (1.0 - K) * sigma2_pred;
end

% Example script
mu = 0.0;
sigma2 = 1.0;
x_goal = 5.0;
sigma_w2 = 0.1;
sigma_v2 = 0.2;
lambda_unc = 0.5;
actions = [-1.0, 0.0, 1.0];

best_u = actions(1);
best_cost = inf;

for k = 1:numel(actions)
    u = actions(k);
    mu_pred = mu + u;
    sigma2_pred = sigma2 + sigma_w2;
    mu_err = mu_pred - x_goal;
    cost = mu_err^2 + lambda_unc * sigma2_pred;
    if cost < best_cost
        best_cost = cost;
        best_u = u;
    end
end

fprintf("Chosen control: %.2f\n", best_u);
      

In Simulink, you can create a subsystem with state variables mu and sigma2, update them with the above equations, and connect the resulting belief to a high-level planner block that computes the cost of candidate controls.

Mathematica is convenient for symbolic derivations and quick numerical experiments with belief-space dynamics. Below is a 1D Gaussian belief update encoded as transformation rules.

ClearAll[PredictBelief1D, UpdateBelief1D, OneStepCost];

PredictBelief1D[{mu_, sigma2_}, u_, sigmaW2_] :=
  {mu + u, sigma2 + sigmaW2};

UpdateBelief1D[{muPred_, sigma2Pred_}, z_, sigmaV2_] :=
 Module[{K, muPost, sigma2Post},
  K = sigma2Pred/(sigma2Pred + sigmaV2);
  muPost = muPred + K (z - muPred);
  sigma2Post = (1 - K) sigma2Pred;
  {muPost, sigma2Post}
 ]

OneStepCost[{mu_, sigma2_}, u_, xGoal_, sigmaW2_, lambdaUnc_] :=
 Module[{muPred, sigma2Pred, muErr},
  {muPred, sigma2Pred} = PredictBelief1D[{mu, sigma2}, u, sigmaW2];
  muErr = muPred - xGoal;
  muErr^2 + lambdaUnc sigma2Pred
 ]

(* Example usage *)
mu0 = 0.0; sigma20 = 1.0;
xGoal = 5.0;
sigmaW2 = 0.1;
lambdaUnc = 0.5;
actions = {-1.0, 0.0, 1.0};

costs = OneStepCost[{mu0, sigma20}, #, xGoal, sigmaW2, lambdaUnc] & /@ actions;
uStar = actions[[First@Ordering[costs, 1]]];

{actions, costs, uStar}
      

Mathematica can also be used to verify properties of belief-space value functions (e.g., convexity for certain finite-state problems) or to prototype small belief-space dynamic programming solvers.

Problem 1 (Discrete Belief Update): Consider a robot with finite state space \( \mathcal{X} = \{1, 2, \dots, N\} \), transition probabilities \( P_{ij}(u) = p(x_{t+1}=j \mid x_t=i, u_t=u) \) and observation likelihoods \( O_j(z) = p(z_t=z \mid x_t=j) \). Show that the belief update can be written as

\[ \bar{b}_{t+1}(j) = \sum_{i=1}^N P_{ij}(u_t)\, b_t(i), \quad b_{t+1}(j) = \eta\, O_j(z_{t+1})\, \bar{b}_{t+1}(j), \]

where \( \eta \) is a normalizing constant.

Solution. By definition \( b_t(i) = p(x_t=i \mid z_{0:t}, u_{0:t-1}) \). The prediction is

\[ \bar{b}_{t+1}(j) = p(x_{t+1}=j \mid z_{0:t}, u_{0:t}) = \sum_{i=1}^N p(x_{t+1}=j \mid x_t=i, u_t)\, p(x_t=i \mid z_{0:t}, u_{0:t-1}), \]

which yields the first expression. For the update,

\[ b_{t+1}(j) = p(x_{t+1}=j \mid z_{0:t+1}, u_{0:t}) \propto p(z_{t+1} \mid x_{t+1}=j)\, p(x_{t+1}=j \mid z_{0:t}, u_{0:t}) = O_j(z_{t+1})\, \bar{b}_{t+1}(j), \]

and normalization gives the required form with \( \eta \).

Problem 2 (Belief Markov Property): Using the discrete setting of Problem 1, prove explicitly that \( p(b_{t+1} \mid b_0, u_0, \dots, b_t, u_t) = p(b_{t+1} \mid b_t, u_t) \).

Solution. The belief \( b_t \) is a distribution on \( \mathcal{X} \). Given \( b_t \) and \( u_t \), the distribution over \( x_{t+1} \) is fixed:

\[ p(x_{t+1}=j \mid b_t, u_t) = \sum_{i=1}^N P_{ij}(u_t)\, b_t(i). \]

The distribution of the next observation is then

\[ p(z_{t+1} \mid b_t, u_t) = \sum_{j=1}^N O_j(z_{t+1})\, p(x_{t+1}=j \mid b_t, u_t). \]

The new belief \( b_{t+1} \) is a deterministic function of \( (b_t, u_t, z_{t+1}) \) via the update formula in Problem 1. Thus, conditioned on \( (b_t, u_t) \), the distribution of \( b_{t+1} \) depends only on the distribution of \( z_{t+1} \), which is already determined by \( (b_t, u_t) \). Past beliefs \( b_0, \dots, b_{t-1} \) do not enter, establishing the claimed Markov property.

Problem 3 (Gaussian Prediction Covariance): For the linear-Gaussian system

\[ x_{t+1} = \mathbf{A} x_t + \mathbf{B} u_t + w_t, \quad w_t \sim \mathcal{N}(\mathbf{0}, \mathbf{Q}), \]

with belief \( x_t \sim \mathcal{N}(\boldsymbol{\mu}_t, \mathbf{\Sigma}_t) \), derive the prediction covariance \( \mathbf{\Sigma}_{t+1}^- \).

Solution. Write \( x_{t+1} = \mathbf{A} x_t + \mathbf{B} u_t + w_t \). Since \( u_t \) is deterministic for prediction, \( \mathbf{B} u_t \) is a constant. The prediction mean is

\[ \boldsymbol{\mu}_{t+1}^- = \mathbb{E}[x_{t+1}] = \mathbf{A}\boldsymbol{\mu}_t + \mathbf{B} u_t. \]

The prediction covariance is

\[ \mathbf{\Sigma}_{t+1}^- = \mathbb{E}\big[(x_{t+1} - \boldsymbol{\mu}_{t+1}^-) (x_{t+1} - \boldsymbol{\mu}_{t+1}^-)^\top\big]. \]

Substitute and use independence of \( x_t \) and \( w_t \):

\[ x_{t+1} - \boldsymbol{\mu}_{t+1}^- = \mathbf{A}(x_t - \boldsymbol{\mu}_t) + w_t, \]

\[ \mathbf{\Sigma}_{t+1}^- = \mathbf{A}\mathbf{\Sigma}_t\mathbf{A}^\top + \mathbb{E}[w_t w_t^\top] = \mathbf{A}\mathbf{\Sigma}_t\mathbf{A}^\top + \mathbf{Q}. \]

Problem 4 (Expected Quadratic Cost for Gaussian Belief): Using the result of Section 5, verify for 1D that if \( x \sim \mathcal{N}(\mu, \sigma^2) \) and \( q > 0 \), then

\[ \mathbb{E}[q (x - x_{\text{goal}})^2] = q (\mu - x_{\text{goal}})^2 + q \sigma^2. \]

Solution. This is a special case of the multivariate identity with \( \mathbf{Q}_c = [q] \), \( \mathbf{\Sigma} = [\sigma^2] \), and scalar \( x \). The trace reduces to \( \mathrm{tr}(\mathbf{Q}_c \mathbf{\Sigma}) = q \sigma^2 \), and the mean term reduces to \( q (\mu - x_{\text{goal}})^2 \), giving the expression.

Problem 5 (One-Step Belief-Space Action Choice): For the 1D example of Section 6 with current belief \( x_t \sim \mathcal{N}(\mu_t, \sigma_t^2) \), process noise variance \( \sigma_w^2 \), and goal \( x_{\text{goal}} \), we approximate the one-step cost as

\[ c_B(b_t, u_t) = (\mu_t + u_t - x_{\text{goal}})^2 + \lambda (\sigma_t^2 + \sigma_w^2). \]

Show that the difference in cost between two actions \( u_a \) and \( u_b \) is independent of \( \lambda \) and of the variances.

Solution. The cost difference is

\[ c_B(b_t, u_a) - c_B(b_t, u_b) = (\mu_t + u_a - x_{\text{goal}})^2 - (\mu_t + u_b - x_{\text{goal}})^2 + \lambda(\sigma_t^2 + \sigma_w^2 - \sigma_t^2 - \sigma_w^2). \]

The variance terms cancel exactly. Therefore

\[ c_B(b_t, u_a) - c_B(b_t, u_b) = (\mu_t + u_a - x_{\text{goal}})^2 - (\mu_t + u_b - x_{\text{goal}})^2, \]

which depends only on \( \mu_t \), \( x_{\text{goal}} \), and the two actions. Hence, in this approximated one-step problem, the choice between actions is driven purely by the predicted mean position, not by uncertainty.

In this lesson we lifted planning from the physical state space to the belief space, the space of probability distributions over the robot state. We showed that beliefs evolve under the Bayes filter and form a Markov process, enabling dynamic programming directly in belief space. In the linear-Gaussian setting, beliefs are Gaussian and evolve via Kalman filter equations, making the belief state finite-dimensional and suitable for optimization. We also discussed how to design belief-dependent costs that trade off goal-reaching and uncertainty reduction, and we implemented minimal belief-space planners in Python, C++, Java, MATLAB/Simulink, and Wolfram Mathematica. These ideas provide the foundation for more advanced chance-constrained and POMDP-based planners in subsequent lessons.