In Chapters 5–6 you built dead-reckoning and Bayes-filter intuition. EKF localization specializes the Bayes filter to a Gaussian belief over the robot pose, updated by: (i) a prediction step that propagates uncertainty through the motion model using odometry/IMU-derived control inputs, and (ii) a correction step that contracts uncertainty using exteroceptive measurements (e.g., GPS, beacons, landmark observations).

We estimate the planar pose in a map/world frame \( \mathbf{x}_k \): \( \mathbf{x}_k = [x_k,\; y_k,\; \theta_k]^\top \), while controls are interpreted as body-frame forward speed and yaw rate \( \mathbf{u}_k \): \( \mathbf{u}_k = [v_k,\; \omega_k]^\top \).

flowchart TD
  A["Initialize: xhat0, P0"] --> B["Loop over time k"]
  B --> C["Read controls u_k (odometry/IMU)"]
  C --> D["Predict: xhat_k_km1, P_k_km1"]
  D --> E{"Measurement available?"}
  E -->|no| H["Publish xhat_k_km1, P_k_km1"]
  E -->|yes| F["Compute innovation y and Jacobians"]
  F --> G["Update: xhat_k_k, P_k_k"]
  G --> H["Publish xhat_k_k, P_k_k"]
  H --> B

The EKF is not “just a set of formulas.” It is a structured approximation: it assumes the belief stays close to Gaussian and relies on local linearization of nonlinear kinematics and sensors. The key engineering questions you must answer in an AMR are: (a) what state and frames to use, (b) what noise models are credible, (c) how to compute Jacobians robustly, and (d) how to handle angle wrapping and numerical stability.

We work in two frames: a world/map frame \( \{W\} \) and a robot body frame \( \{B\} \). The state \( \mathbf{x}_k \) is expressed in \( \{W\} \). The control \( \mathbf{u}_k \) is interpreted as body-frame forward velocity and yaw rate (typical for ground AMR odometry pipelines).

The nonlinear discrete-time stochastic system is:

\[ \mathbf{x}_{k+1} \; ← \; f(\mathbf{x}_k,\mathbf{u}_k) \;+\; \mathbf{w}_k, \quad \mathbf{w}_k \sim \mathcal{N}(\mathbf{0},\mathbf{Q}_k) \]

\[ \mathbf{z}_k \; ← \; h(\mathbf{x}_k) \;+\; \mathbf{v}_k, \quad \mathbf{v}_k \sim \mathcal{N}(\mathbf{0},\mathbf{R}_k) \]

The EKF maintains a Gaussian belief: \( b_k(\mathbf{x}) \): \( p(\mathbf{x}_k \mid \mathbf{z}_{1:k},\mathbf{u}_{1:k}) \approx \mathcal{N}(\hat{\mathbf{x}}_{k|k}, \mathbf{P}_{k|k}) \).

Mobile-robot specifics (why framing matters): wheel odometry naturally produces incremental motion in \( \{B\} \). Expressing \( \hat{\mathbf{x}} \) in \( \{W\} \) forces the motion model to rotate body-frame velocity by heading \( \theta \), making nonlinearity unavoidable even in planar motion.

Before linearization, we establish the “exact” baseline: for linear dynamics and linear measurements with Gaussian noise, the Bayes filter closes exactly over Gaussians.

Consider: \( \mathbf{x}_k \in \mathbb{R}^n \), \( \mathbf{z}_k \in \mathbb{R}^m \),

\[ \mathbf{x}_{k+1} \; ← \; \mathbf{A}_k \mathbf{x}_k + \mathbf{B}_k \mathbf{u}_k + \mathbf{w}_k, \quad \mathbf{w}_k \sim \mathcal{N}(\mathbf{0},\mathbf{Q}_k) \]

\[ \mathbf{z}_k \; ← \; \mathbf{H}_k \mathbf{x}_k + \mathbf{v}_k, \quad \mathbf{v}_k \sim \mathcal{N}(\mathbf{0},\mathbf{R}_k) \]

Theorem (Gaussian closure): If \( p(\mathbf{x}_k \mid \mathbf{z}_{1:k}) \) is Gaussian, then both \( p(\mathbf{x}_{k+1} \mid \mathbf{z}_{1:k}) \) (prediction) and \( p(\mathbf{x}_{k+1} \mid \mathbf{z}_{1:k+1}) \) (update) are Gaussian.

Proof (sketch with explicit algebraic structure):

Prediction is an affine transformation of a Gaussian plus independent Gaussian noise; hence it is Gaussian. For the update, Bayes’ rule yields (up to normalization):

\[ p(\mathbf{x}_k \mid \mathbf{z}_k) \;\propto\; \exp\!\Big( -\tfrac{1}{2}(\mathbf{x}_k-\hat{\mathbf{x}}_{k|k-1})^\top \mathbf{P}_{k|k-1}^{-1}(\mathbf{x}_k-\hat{\mathbf{x}}_{k|k-1}) -\tfrac{1}{2}(\mathbf{z}_k-\mathbf{H}_k\mathbf{x}_k)^\top \mathbf{R}_k^{-1}(\mathbf{z}_k-\mathbf{H}_k\mathbf{x}_k) \Big). \]

The exponent is a quadratic form in \( \mathbf{x}_k \); completing the square produces a Gaussian posterior with mean and covariance:

\[ \mathbf{K}_k \; ← \; \mathbf{P}_{k|k-1}\mathbf{H}_k^\top\left(\mathbf{H}_k\mathbf{P}_{k|k-1}\mathbf{H}_k^\top+\mathbf{R}_k\right)^{-1}, \quad \hat{\mathbf{x}}_{k|k} \; ← \; \hat{\mathbf{x}}_{k|k-1} + \mathbf{K}_k(\mathbf{z}_k-\mathbf{H}_k\hat{\mathbf{x}}_{k|k-1}) \]

\[ \mathbf{P}_{k|k} \; ← \; (\mathbf{I}-\mathbf{K}_k\mathbf{H}_k)\mathbf{P}_{k|k-1}. \]

This exact result is what the EKF imitates after local linearization.

EKF replaces nonlinear models with first-order Taylor approximations around the current estimate. Define Jacobians (evaluated at the current linearization point): \( \mathbf{F}_k \): \( \mathbf{F}_k = \frac{\partial f}{\partial \mathbf{x}}\big|_{\hat{\mathbf{x}}_{k|k},\mathbf{u}_k} \), \( \mathbf{H}_k \): \( \mathbf{H}_k = \frac{\partial h}{\partial \mathbf{x}}\big|_{\hat{\mathbf{x}}_{k|k-1}} \).

Linearize the motion model around \( \hat{\mathbf{x}}_{k|k} \):

\[ f(\mathbf{x}_k,\mathbf{u}_k) \;\approx\; f(\hat{\mathbf{x}}_{k|k},\mathbf{u}_k) \;+\; \mathbf{F}_k(\mathbf{x}_k-\hat{\mathbf{x}}_{k|k}). \]

Prediction step:

\[ \hat{\mathbf{x}}_{k+1|k} \; ← \; f(\hat{\mathbf{x}}_{k|k},\mathbf{u}_k) \]

\[ \mathbf{P}_{k+1|k} \; ← \; \mathbf{F}_k \mathbf{P}_{k|k}\mathbf{F}_k^\top \;+\; \mathbf{L}_k \mathbf{Q}_k \mathbf{L}_k^\top \]

where \( \mathbf{L}_k \) models how process noise enters the state (e.g., control noise).

Update step: Linearize measurement around \( \hat{\mathbf{x}}_{k|k-1} \):

\[ h(\mathbf{x}_k) \;\approx\; h(\hat{\mathbf{x}}_{k|k-1}) \;+\; \mathbf{H}_k(\mathbf{x}_k-\hat{\mathbf{x}}_{k|k-1}). \]

\[ \mathbf{S}_k \; ← \; \mathbf{H}_k\mathbf{P}_{k|k-1}\mathbf{H}_k^\top + \mathbf{R}_k, \quad \mathbf{K}_k \; ← \; \mathbf{P}_{k|k-1}\mathbf{H}_k^\top \mathbf{S}_k^{-1} \]

\[ \hat{\mathbf{x}}_{k|k} \; ← \; \hat{\mathbf{x}}_{k|k-1} + \mathbf{K}_k\left(\mathbf{z}_k - h(\hat{\mathbf{x}}_{k|k-1})\right). \]

Numerically stable covariance update (Joseph form):

\[ \mathbf{P}_{k|k} \; ← \; (\mathbf{I}-\mathbf{K}_k\mathbf{H}_k)\mathbf{P}_{k|k-1}(\mathbf{I}-\mathbf{K}_k\mathbf{H}_k)^\top + \mathbf{K}_k\mathbf{R}_k\mathbf{K}_k^\top. \]

flowchart TD
  U["u_k"] --> PRED["Predict"]
  XH["x_hat_k|k, P_k|k"] --> PRED
  PRED --> XP["x_hat_k+1|k, P_k+1|k"]
  Z["z_k+1"] --> UPD["Update"]
  XP --> UPD
  UPD --> XO["x_hat_k+1|k+1, P_k+1|k+1"]
  UPD --> INNOV["innovation y = z - h(x_hat)"]
        

Interpretation: EKF is “Kalman algebra” applied to the locally linearized system. The quality of the approximation depends on how accurate the first-order expansion is in the region where the posterior has mass.

We adopt a standard planar velocity motion model (appropriate when odometry provides forward speed \( v_k \) and yaw rate \( \omega_k \)). For sampling period \( \Delta t \):

\[ \begin{aligned} 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} \; ← \; \mathrm{wrap}(\theta_k + \Delta t\, \omega_k) \end{aligned} \]

Let control noise be \( \mathbf{n}_k = [n_{v,k},\; n_{\omega,k}]^\top \sim \mathcal{N}(\mathbf{0}, \mathbf{Q}_k) \) and enter additively into controls: \( v_k \mapsto v_k + n_{v,k} \), \( \omega_k \mapsto \omega_k + n_{\omega,k} \). Then the noise injection Jacobian is:

\[ \mathbf{L}_k \; ← \; \begin{bmatrix} \Delta t\cos\theta_k & 0 \\ \Delta t\sin\theta_k & 0 \\ 0 & \Delta t \end{bmatrix}. \]

Jacobian \( \mathbf{F}_k \) derivation: compute partial derivatives of \( f(\mathbf{x}_k,\mathbf{u}_k) \) w.r.t. \( \mathbf{x}_k \). The only nontrivial terms are those depending on \( \theta_k \).

\[ \mathbf{F}_k \; ← \; \frac{\partial f}{\partial \mathbf{x}} \;=\; \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}. \]

Now consider a known landmark at \( \mathbf{m}_i = [m_{x,i}, m_{y,i}]^\top \) in the world frame and a range–bearing sensor:

\[ \mathbf{z}_{k,i} \; ← \; \begin{bmatrix} r_{k,i} \\ \phi_{k,i} \end{bmatrix} \;=\; \begin{bmatrix} \sqrt{(m_{x,i}-x_k)^2 + (m_{y,i}-y_k)^2} \\ \mathrm{wrap}\!\left(\mathrm{atan2}(m_{y,i}-y_k,\; m_{x,i}-x_k) - \theta_k\right) \end{bmatrix} + \mathbf{v}_{k,i}. \]

Define \( d_x = m_x - x \), \( d_y = m_y - y \), \( q = d_x^2 + d_y^2 \), \( r = \sqrt{q} \). Then the measurement Jacobian \( \mathbf{H}_{k,i} = \frac{\partial h_i}{\partial \mathbf{x}} \) is:

\[ \mathbf{H}_{k,i} \; ← \; \begin{bmatrix} -d_x/r & -d_y/r & 0 \\ d_y/q & -d_x/q & -1 \end{bmatrix}. \]

Engineering note: the bearing innovation must be wrapped to \( (-\pi, \pi] \); otherwise, the EKF can apply a large, incorrect correction when the true residual crosses the \( \pm\pi \) boundary.

In AMR systems, different sensors arrive at different rates (e.g., wheel odometry at 50–200 Hz, IMU at 100–400 Hz, GPS at 1–20 Hz, landmark observations sporadically). EKF accommodates this naturally:

• Run predict whenever a new control increment is available.
• Run update whenever a measurement arrives (one at a time, in timestamp order).

If multiple independent measurements share the same timestamp (or are close enough to treat as simultaneous), you can update sequentially with \( (\mathbf{z}_1,\mathbf{R}_1), (\mathbf{z}_2,\mathbf{R}_2), \dots \). For the linear case, sequential and batch updates are equivalent; the EKF adopts sequential updates as a practical approximation.

A common pattern is to treat each measurement channel as producing a residual: \( \mathbf{y}_k \): \( \mathbf{y}_k = \mathbf{z}_k - h(\hat{\mathbf{x}}_{k|k-1}) \), and decide whether to accept it using a normalized innovation statistic:

\[ \gamma_k \; ← \; \mathbf{y}_k^\top \mathbf{S}_k^{-1}\mathbf{y}_k, \quad \text{accept if } \gamma_k \; < \; \eta \]

where \( \eta \) is a threshold chosen from a chi-square distribution for the measurement dimension (a standard statistical gating heuristic).

File: Chapter7_Lesson1.py

# Chapter7_Lesson1.py
"""
Chapter 7 - Lesson 1: EKF Localization Pipeline (mobile framing)

A minimal, from-scratch EKF localization demo:
- State: x = [x, y, theta]^T in world frame W
- Control: u = [v, omega]^T measured in body frame B
- Measurement: range-bearing to known landmarks (2D)

Dependencies: numpy, matplotlib (optional for plotting)
"""

from __future__ import annotations
import math
import numpy as np

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

def motion_model(x: np.ndarray, u: np.ndarray, dt: float) -> np.ndarray:
    """
    x_{k+1} = f(x_k, u_k) (deterministic part)
    x = [x, y, theta], u = [v, omega]
    """
    px, py, th = float(x[0]), float(x[1]), float(x[2])
    v, w = float(u[0]), float(u[1])
    nx = np.zeros((3, 1), dtype=float)
    nx[0, 0] = px + dt * v * math.cos(th)
    nx[1, 0] = py + dt * v * math.sin(th)
    nx[2, 0] = wrap_angle(th + dt * w)
    return nx

def jacobian_F(x: np.ndarray, u: np.ndarray, dt: float) -> np.ndarray:
    """F_k = d f / d x evaluated at (x,u)."""
    th = float(x[2])
    v = float(u[0])
    F = np.eye(3, dtype=float)
    F[0, 2] = -dt * v * math.sin(th)
    F[1, 2] =  dt * v * math.cos(th)
    return F

def jacobian_L(x: np.ndarray, dt: float) -> np.ndarray:
    """
    L_k = d f / d w where process noise w = [n_v, n_omega]^T
    added to control: v_hat = v + n_v, omega_hat = omega + n_omega
    """
    th = float(x[2])
    L = np.zeros((3, 2), dtype=float)
    L[0, 0] = dt * math.cos(th)
    L[1, 0] = dt * math.sin(th)
    L[2, 1] = dt
    return L

def meas_model_range_bearing(x: np.ndarray, lm: np.ndarray) -> np.ndarray:
    """
    z = h(x) = [range, bearing]^T to a landmark lm=[mx,my]^T in world frame.
    bearing is relative to robot heading theta.
    """
    px, py, th = float(x[0]), float(x[1]), float(x[2])
    mx, my = float(lm[0]), float(lm[1])
    dx = mx - px
    dy = my - py
    r = math.sqrt(dx*dx + dy*dy)
    b = wrap_angle(math.atan2(dy, dx) - th)
    return np.array([[r], [b]], dtype=float)

def jacobian_H_range_bearing(x: np.ndarray, lm: np.ndarray) -> np.ndarray:
    """H = d h / d x for range-bearing."""
    px, py, th = float(x[0]), float(x[1]), float(x[2])
    mx, my = float(lm[0]), float(lm[1])
    dx = mx - px
    dy = my - py
    q = dx*dx + dy*dy
    r = math.sqrt(q)
    # Guard against division by zero
    eps = 1e-12
    q = max(q, eps)
    r = max(r, eps)

    H = np.zeros((2, 3), dtype=float)
    # range derivatives
    H[0, 0] = -dx / r
    H[0, 1] = -dy / r
    H[0, 2] = 0.0
    # bearing derivatives
    H[1, 0] =  dy / q
    H[1, 1] = -dx / q
    H[1, 2] = -1.0
    return H

class EKFLocalizer:
    def __init__(self, x0: np.ndarray, P0: np.ndarray):
        self.x = x0.reshape(3, 1).astype(float)
        self.P = P0.astype(float)

    def predict(self, u: np.ndarray, dt: float, Q: np.ndarray):
        """
        EKF prediction:
        x <- f(x,u)
        P <- F P F^T + L Q L^T
        """
        F = jacobian_F(self.x, u, dt)
        L = jacobian_L(self.x, dt)
        self.x = motion_model(self.x, u, dt)
        self.P = F @ self.P @ F.T + L @ Q @ L.T
        # Enforce symmetry
        self.P = 0.5 * (self.P + self.P.T)

    def update_range_bearing(self, z: np.ndarray, lm: np.ndarray, R: np.ndarray):
        """
        EKF measurement update for range-bearing:
        y = z - h(x)
        S = H P H^T + R
        K = P H^T S^{-1}
        x <- x + K y
        P <- (I - K H) P (I - K H)^T + K R K^T (Joseph form)
        """
        H = jacobian_H_range_bearing(self.x, lm)
        zhat = meas_model_range_bearing(self.x, lm)
        y = z.reshape(2, 1) - zhat
        y[1, 0] = wrap_angle(float(y[1, 0]))  # bearing innovation

        S = H @ self.P @ H.T + R
        K = self.P @ H.T @ np.linalg.inv(S)

        self.x = self.x + K @ y
        self.x[2, 0] = wrap_angle(float(self.x[2, 0]))

        I = np.eye(3, dtype=float)
        self.P = (I - K @ H) @ self.P @ (I - K @ H).T + K @ R @ K.T
        self.P = 0.5 * (self.P + self.P.T)

def run_demo(seed: int = 7, do_plot: bool = False):
    np.random.seed(seed)

    # Landmarks (world frame)
    landmarks = np.array([
        [5.0,  0.0],
        [5.0,  5.0],
        [0.0,  5.0],
        [-3.0, 2.0],
    ], dtype=float)

    # Simulation parameters
    dt = 0.1
    N = 250

    # True process noise (on control) and assumed process noise in EKF
    sigma_v_true = 0.05
    sigma_w_true = 0.03
    Q = np.diag([0.08**2, 0.05**2])  # assumed (tunable)

    # Measurement noise (range, bearing)
    R = np.diag([0.15**2, (2.0*math.pi/180.0)**2])

    # Initial truth and estimate
    x_true = np.array([[0.0], [0.0], [0.0]])
    x0 = np.array([[-0.2], [0.1], [0.05]])
    P0 = np.diag([0.5**2, 0.5**2, (10.0*math.pi/180.0)**2])

    ekf = EKFLocalizer(x0, P0)

    # Logging
    xs_true = np.zeros((N, 3))
    xs_est = np.zeros((N, 3))

    for k in range(N):
        # Control command (body frame): a gentle curve
        v_cmd = 0.8 + 0.2 * math.sin(0.04 * k)
        w_cmd = 0.25 + 0.05 * math.cos(0.03 * k)
        u_cmd = np.array([[v_cmd], [w_cmd]])

        # Apply noise to control for truth evolution
        u_true = np.array([
            [v_cmd + np.random.normal(0.0, sigma_v_true)],
            [w_cmd + np.random.normal(0.0, sigma_w_true)],
        ], dtype=float)

        # True motion
        x_true = motion_model(x_true, u_true, dt)

        # EKF predict uses commanded control
        ekf.predict(u_cmd, dt, Q)

        # Measurement: pick nearest landmark (simple association)
        dists = np.linalg.norm(landmarks - x_true[:2, 0].reshape(1, 2), axis=1)
        idx = int(np.argmin(dists))
        lm = landmarks[idx]

        z_true = meas_model_range_bearing(x_true, lm)
        z_meas = np.array([
            [z_true[0, 0] + np.random.normal(0.0, math.sqrt(R[0, 0]))],
            [wrap_angle(z_true[1, 0] + np.random.normal(0.0, math.sqrt(R[1, 1])))]
        ], dtype=float)

        # EKF update
        ekf.update_range_bearing(z_meas, lm, R)

        xs_true[k, :] = x_true[:, 0]
        xs_est[k, :] = ekf.x[:, 0]

    # Report RMS error (position + heading)
    pos_err = xs_est[:, :2] - xs_true[:, :2]
    th_err = np.array([wrap_angle(a) for a in (xs_est[:, 2] - xs_true[:, 2])])
    rms_pos = float(np.sqrt(np.mean(np.sum(pos_err**2, axis=1))))
    rms_th = float(np.sqrt(np.mean(th_err**2)))

    print("RMS position error [m]:", rms_pos)
    print("RMS heading error [rad]:", rms_th)

    if do_plot:
        import matplotlib.pyplot as plt
        plt.figure()
        plt.plot(xs_true[:, 0], xs_true[:, 1], label="truth")
        plt.plot(xs_est[:, 0], xs_est[:, 1], label="ekf")
        plt.scatter(landmarks[:, 0], landmarks[:, 1], marker="x", label="landmarks")
        plt.axis("equal")
        plt.legend()
        plt.title("EKF localization demo")
        plt.show()

if __name__ == "__main__":
    run_demo(do_plot=False)

Robotics-oriented Python libraries (context): in production stacks, EKF logic often appears in ROS/ROS2 nodes (e.g., multi-sensor fusion pipelines). For research prototyping, you will commonly see numpy/scipy, symbolic Jacobians via sympy, and in some cases EKF utilities in filterpy. This lesson keeps the implementation explicit.

File: Chapter7_Lesson1.cpp

// Chapter7_Lesson1.cpp
// Chapter 7 - Lesson 1: EKF Localization Pipeline (mobile framing)
//
// Minimal EKF localization demo (2D) using Eigen.
// Build example (Linux):
//   g++ -O2 -std=c++17 Chapter7_Lesson1.cpp -I/usr/include/eigen3 -o ekf_demo
//
// State: [x, y, theta]^T (world frame), control: [v, omega]^T (body frame)
// Measurement: range-bearing to known landmark.

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

struct Landmark {
  double mx;
  double my;
};

static double wrapAngle(double a) {
  const double twoPi = 2.0 * M_PI;
  a = std::fmod(a + M_PI, twoPi);
  if (a < 0) a += twoPi;
  return a - M_PI;
}

static Eigen::Vector3d motionModel(const Eigen::Vector3d& x,
                                   const Eigen::Vector2d& u,
                                   double dt) {
  const double px = x(0), py = x(1), th = x(2);
  const double v = u(0), w = u(1);

  Eigen::Vector3d nx;
  nx(0) = px + dt * v * std::cos(th);
  nx(1) = py + dt * v * std::sin(th);
  nx(2) = wrapAngle(th + dt * w);
  return nx;
}

static Eigen::Matrix3d jacobianF(const Eigen::Vector3d& x,
                                 const Eigen::Vector2d& u,
                                 double dt) {
  const double th = x(2);
  const double v = u(0);
  Eigen::Matrix3d F = Eigen::Matrix3d::Identity();
  F(0,2) = -dt * v * std::sin(th);
  F(1,2) =  dt * v * std::cos(th);
  return F;
}

static Eigen::Matrix<double,3,2> jacobianL(const Eigen::Vector3d& x, double dt) {
  const double th = x(2);
  Eigen::Matrix<double,3,2> L;
  L.setZero();
  L(0,0) = dt * std::cos(th);
  L(1,0) = dt * std::sin(th);
  L(2,1) = dt;
  return L;
}

static Eigen::Vector2d measRangeBearing(const Eigen::Vector3d& x, const Landmark& lm) {
  const double dx = lm.mx - x(0);
  const double dy = lm.my - x(1);
  const double r = std::sqrt(dx*dx + dy*dy);
  const double b = wrapAngle(std::atan2(dy, dx) - x(2));
  return Eigen::Vector2d(r, b);
}

static Eigen::Matrix<double,2,3> jacobianH(const Eigen::Vector3d& x, const Landmark& lm) {
  const double dx = lm.mx - x(0);
  const double dy = lm.my - x(1);
  double q = dx*dx + dy*dy;
  const double eps = 1e-12;
  if (q < eps) q = eps;
  const double r = std::sqrt(q);

  Eigen::Matrix<double,2,3> H;
  H.setZero();
  // range
  H(0,0) = -dx / r;
  H(0,1) = -dy / r;
  // bearing
  H(1,0) =  dy / q;
  H(1,1) = -dx / q;
  H(1,2) = -1.0;
  return H;
}

int main() {
  // Landmarks
  std::vector<Landmark> lms = { {5.0,0.0},{5.0,5.0},{0.0,5.0},{-3.0,2.0} };

  const double dt = 0.1;
  const int N = 250;

  // Noise parameters
  const double sigma_v_true = 0.05;
  const double sigma_w_true = 0.03;

  Eigen::Matrix2d Q = Eigen::Matrix2d::Zero();
  Q(0,0) = 0.08*0.08;
  Q(1,1) = 0.05*0.05;

  Eigen::Matrix2d R = Eigen::Matrix2d::Zero();
  R(0,0) = 0.15*0.15;
  R(1,1) = std::pow(2.0*M_PI/180.0, 2);

  // Initial truth / estimate
  Eigen::Vector3d x_true(0.0, 0.0, 0.0);
  Eigen::Vector3d x_est(-0.2, 0.1, 0.05);

  Eigen::Matrix3d P = Eigen::Matrix3d::Zero();
  P(0,0) = 0.5*0.5;
  P(1,1) = 0.5*0.5;
  P(2,2) = std::pow(10.0*M_PI/180.0, 2);

  std::mt19937 rng(7);
  std::normal_distribution<double> nv(0.0, sigma_v_true);
  std::normal_distribution<double> nw(0.0, sigma_w_true);
  std::normal_distribution<double> nr(0.0, std::sqrt(R(0,0)));
  std::normal_distribution<double> nb(0.0, std::sqrt(R(1,1)));

  auto nearestLandmarkIndex = [&](const Eigen::Vector3d& x)->int{
    int best = 0;
    double bestd = 1e100;
    for (int i=0;i<(int)lms.size();++i) {
      const double dx = lms[i].mx - x(0);
      const double dy = lms[i].my - x(1);
      const double d = std::sqrt(dx*dx + dy*dy);
      if (d < bestd) { bestd = d; best = i; }
    }
    return best;
  };

  double sumPos2 = 0.0, sumTh2 = 0.0;

  for (int k=0;k<N;++k) {
    const double v_cmd = 0.8 + 0.2 * std::sin(0.04 * k);
    const double w_cmd = 0.25 + 0.05 * std::cos(0.03 * k);

    Eigen::Vector2d u_cmd(v_cmd, w_cmd);
    Eigen::Vector2d u_true(v_cmd + nv(rng), w_cmd + nw(rng));

    // True propagation
    x_true = motionModel(x_true, u_true, dt);

    // EKF predict
    Eigen::Matrix3d F = jacobianF(x_est, u_cmd, dt);
    Eigen::Matrix<double,3,2> L = jacobianL(x_est, dt);
    x_est = motionModel(x_est, u_cmd, dt);
    P = F * P * F.transpose() + L * Q * L.transpose();
    P = 0.5 * (P + P.transpose());

    // Measurement (nearest landmark)
    const int idx = nearestLandmarkIndex(x_true);
    const Landmark lm = lms[idx];

    Eigen::Vector2d z_true = measRangeBearing(x_true, lm);
    Eigen::Vector2d z_meas(z_true(0) + nr(rng), wrapAngle(z_true(1) + nb(rng)));

    // EKF update
    Eigen::Matrix<double,2,3> H = jacobianH(x_est, lm);
    Eigen::Vector2d zhat = measRangeBearing(x_est, lm);
    Eigen::Vector2d y = z_meas - zhat;
    y(1) = wrapAngle(y(1));

    Eigen::Matrix2d S = H * P * H.transpose() + R;
    Eigen::Matrix<double,3,2> K = P * H.transpose() * S.inverse();

    x_est = x_est + K * y;
    x_est(2) = wrapAngle(x_est(2));

    Eigen::Matrix3d I = Eigen::Matrix3d::Identity();
    P = (I - K*H) * P * (I - K*H).transpose() + K * R * K.transpose();
    P = 0.5 * (P + P.transpose());

    // Errors
    const double ex = x_est(0) - x_true(0);
    const double ey = x_est(1) - x_true(1);
    const double eth = wrapAngle(x_est(2) - x_true(2));
    sumPos2 += ex*ex + ey*ey;
    sumTh2 += eth*eth;
  }

  const double rmsPos = std::sqrt(sumPos2 / (double)N);
  const double rmsTh  = std::sqrt(sumTh2 / (double)N);

  std::cout << "RMS position error [m]: " << rmsPos << "\n";
  std::cout << "RMS heading error [rad]: " << rmsTh << "\n";
  return 0;
}

Robotics-oriented C++ libraries (context): EKF localization commonly uses Eigen for linear algebra, and in ROS2 ecosystems, multi-sensor EKF fusion is often deployed via established packages (while keeping careful track of frames). This lesson focuses on the observer mathematics and a transparent, dependency-light implementation.

File: Chapter7_Lesson1.java

// Chapter7_Lesson1.java
// Chapter 7 - Lesson 1: EKF Localization Pipeline (mobile framing)
//
// Minimal EKF localization demo (2D) in Java using EJML.
// Dependency: org.ejml:ejml-simple
//
// Compile/run (example with Gradle or Maven):
// - Add EJML dependency and run this class' main().

import org.ejml.simple.SimpleMatrix;

import java.util.Random;

public class Chapter7_Lesson1 {

    static class Landmark {
        final double mx, my;
        Landmark(double mx, double my) { this.mx = mx; this.my = my; }
    }

    static double wrapAngle(double a) {
        double twoPi = 2.0 * Math.PI;
        a = (a + Math.PI) % twoPi;
        if (a < 0) a += twoPi;
        return a - Math.PI;
    }

    static SimpleMatrix motionModel(SimpleMatrix x, SimpleMatrix u, double dt) {
        double px = x.get(0), py = x.get(1), th = x.get(2);
        double v = u.get(0), w = u.get(1);
        SimpleMatrix nx = new SimpleMatrix(3,1);
        nx.set(0, px + dt * v * Math.cos(th));
        nx.set(1, py + dt * v * Math.sin(th));
        nx.set(2, wrapAngle(th + dt * w));
        return nx;
    }

    static SimpleMatrix jacobianF(SimpleMatrix x, SimpleMatrix u, double dt) {
        double th = x.get(2);
        double v  = u.get(0);
        SimpleMatrix F = SimpleMatrix.identity(3);
        F.set(0,2, -dt * v * Math.sin(th));
        F.set(1,2,  dt * v * Math.cos(th));
        return F;
    }

    static SimpleMatrix jacobianL(SimpleMatrix x, double dt) {
        double th = x.get(2);
        SimpleMatrix L = new SimpleMatrix(3,2);
        L.set(0,0, dt * Math.cos(th));
        L.set(1,0, dt * Math.sin(th));
        L.set(2,1, dt);
        return L;
    }

    static SimpleMatrix measRangeBearing(SimpleMatrix x, Landmark lm) {
        double dx = lm.mx - x.get(0);
        double dy = lm.my - x.get(1);
        double r = Math.sqrt(dx*dx + dy*dy);
        double b = wrapAngle(Math.atan2(dy, dx) - x.get(2));
        SimpleMatrix z = new SimpleMatrix(2,1);
        z.set(0, r);
        z.set(1, b);
        return z;
    }

    static SimpleMatrix jacobianH(SimpleMatrix x, Landmark lm) {
        double dx = lm.mx - x.get(0);
        double dy = lm.my - x.get(1);
        double q  = dx*dx + dy*dy;
        double eps = 1e-12;
        if (q < eps) q = eps;
        double r = Math.sqrt(q);

        SimpleMatrix H = new SimpleMatrix(2,3);
        // range
        H.set(0,0, -dx / r);
        H.set(0,1, -dy / r);
        // bearing
        H.set(1,0,  dy / q);
        H.set(1,1, -dx / q);
        H.set(1,2, -1.0);
        return H;
    }

    static int nearestLandmarkIndex(SimpleMatrix x, Landmark[] lms) {
        int best = 0;
        double bestd = 1e100;
        for (int i=0;i<lms.length;i++) {
            double dx = lms[i].mx - x.get(0);
            double dy = lms[i].my - x.get(1);
            double d = Math.sqrt(dx*dx + dy*dy);
            if (d < bestd) { bestd = d; best = i; }
        }
        return best;
    }

    public static void main(String[] args) {
        Landmark[] lms = new Landmark[] {
                new Landmark(5.0, 0.0),
                new Landmark(5.0, 5.0),
                new Landmark(0.0, 5.0),
                new Landmark(-3.0, 2.0)
        };

        double dt = 0.1;
        int N = 250;

        // Assumed process noise (tuning)
        SimpleMatrix Q = new SimpleMatrix(2,2);
        Q.set(0,0, 0.08*0.08);
        Q.set(1,1, 0.05*0.05);

        // Measurement noise
        SimpleMatrix R = new SimpleMatrix(2,2);
        R.set(0,0, 0.15*0.15);
        R.set(1,1, Math.pow(2.0*Math.PI/180.0, 2));

        // True noise (on control)
        double sigmaVTrue = 0.05;
        double sigmaWTrue = 0.03;

        Random rng = new Random(7);

        // Initial truth and estimate
        SimpleMatrix xTrue = new SimpleMatrix(3,1);
        SimpleMatrix xEst  = new SimpleMatrix(3,1);
        xEst.set(0, -0.2); xEst.set(1, 0.1); xEst.set(2, 0.05);

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

        double sumPos2 = 0.0, sumTh2 = 0.0;

        for (int k=0;k<N;k++) {
            double vCmd = 0.8 + 0.2 * Math.sin(0.04 * k);
            double wCmd = 0.25 + 0.05 * Math.cos(0.03 * k);

            SimpleMatrix uCmd = new SimpleMatrix(2,1);
            uCmd.set(0, vCmd); uCmd.set(1, wCmd);

            // Truth uses noisy control
            double vTrue = vCmd + rng.nextGaussian() * sigmaVTrue;
            double wTrue = wCmd + rng.nextGaussian() * sigmaWTrue;
            SimpleMatrix uTrue = new SimpleMatrix(2,1);
            uTrue.set(0, vTrue); uTrue.set(1, wTrue);

            xTrue = motionModel(xTrue, uTrue, dt);

            // EKF predict
            SimpleMatrix F = jacobianF(xEst, uCmd, dt);
            SimpleMatrix L = jacobianL(xEst, dt);
            xEst = motionModel(xEst, uCmd, dt);
            P = F.mult(P).mult(F.transpose()).plus(L.mult(Q).mult(L.transpose()));
            P = P.plus(P.transpose()).scale(0.5);

            // Measurement (nearest landmark)
            int idx = nearestLandmarkIndex(xTrue, lms);
            Landmark lm = lms[idx];

            SimpleMatrix zTrue = measRangeBearing(xTrue, lm);
            double zR = zTrue.get(0) + rng.nextGaussian() * Math.sqrt(R.get(0,0));
            double zB = wrapAngle(zTrue.get(1) + rng.nextGaussian() * Math.sqrt(R.get(1,1)));
            SimpleMatrix zMeas = new SimpleMatrix(2,1);
            zMeas.set(0, zR); zMeas.set(1, zB);

            // EKF update
            SimpleMatrix H = jacobianH(xEst, lm);
            SimpleMatrix zHat = measRangeBearing(xEst, lm);
            SimpleMatrix y = zMeas.minus(zHat);
            y.set(1, wrapAngle(y.get(1)));

            SimpleMatrix S = H.mult(P).mult(H.transpose()).plus(R);
            SimpleMatrix K = P.mult(H.transpose()).mult(S.invert());

            xEst = xEst.plus(K.mult(y));
            xEst.set(2, wrapAngle(xEst.get(2)));

            SimpleMatrix I = SimpleMatrix.identity(3);
            P = (I.minus(K.mult(H))).mult(P).mult(I.minus(K.mult(H)).transpose())
                    .plus(K.mult(R).mult(K.transpose()));
            P = P.plus(P.transpose()).scale(0.5);

            // Errors
            double ex = xEst.get(0) - xTrue.get(0);
            double ey = xEst.get(1) - xTrue.get(1);
            double eth = wrapAngle(xEst.get(2) - xTrue.get(2));
            sumPos2 += ex*ex + ey*ey;
            sumTh2 += eth*eth;
        }

        double rmsPos = Math.sqrt(sumPos2 / (double)N);
        double rmsTh  = Math.sqrt(sumTh2 / (double)N);

        System.out.println("RMS position error [m]: " + rmsPos);
        System.out.println("RMS heading error [rad]: " + rmsTh);
    }
}

Robotics-oriented Java libraries (context): EJML is a common choice for matrix algebra in Java robotics. If you work in ROS-adjacent Java ecosystems, matrix backends such as EJML are frequently paired with middleware bindings. This lesson keeps the estimator core explicit.

File: Chapter7_Lesson1.m

% Chapter7_Lesson1.m
% Chapter 7 - Lesson 1: EKF Localization Pipeline (mobile framing)
%
% Minimal EKF localization demo (2D) in MATLAB (from scratch).
% State: [x; y; theta] in world frame, Control: [v; omega] in body frame
% Measurement: range-bearing to known landmarks.

function Chapter7_Lesson1()
    rng(7);

    landmarks = [ 5 0;
                  5 5;
                  0 5;
                 -3 2 ];

    dt = 0.1;
    N  = 250;

    % True noise on control (simulation)
    sigma_v_true = 0.05;
    sigma_w_true = 0.03;

    % Assumed process noise in EKF (tuning)
    Q = diag([0.08^2, 0.05^2]);

    % Measurement noise (range, bearing)
    R = diag([0.15^2, (2*pi/180)^2]);

    % Initial truth and estimate
    x_true = [0; 0; 0];
    x_est  = [-0.2; 0.1; 0.05];
    P      = diag([0.5^2, 0.5^2, (10*pi/180)^2]);

    sumPos2 = 0;
    sumTh2  = 0;

    for k = 1:N
        v_cmd = 0.8 + 0.2*sin(0.04*(k-1));
        w_cmd = 0.25 + 0.05*cos(0.03*(k-1));
        u_cmd = [v_cmd; w_cmd];

        u_true = [v_cmd + sigma_v_true*randn();
                  w_cmd + sigma_w_true*randn()];

        % True propagation
        x_true = motion_model(x_true, u_true, dt);

        % EKF predict
        F = jacobian_F(x_est, u_cmd, dt);
        L = jacobian_L(x_est, dt);
        x_est = motion_model(x_est, u_cmd, dt);
        P = F*P*F' + L*Q*L';
        P = 0.5*(P + P');

        % Measurement (nearest landmark)
        idx = nearest_landmark(x_true, landmarks);
        lm  = landmarks(idx,:);

        z_true = meas_model(x_true, lm);
        z_meas = [z_true(1) + sqrt(R(1,1))*randn();
                  wrap_angle(z_true(2) + sqrt(R(2,2))*randn())];

        % EKF update
        H = jacobian_H(x_est, lm);
        zhat = meas_model(x_est, lm);

        y = z_meas - zhat;
        y(2) = wrap_angle(y(2));

        S = H*P*H' + R;
        K = P*H'/S;

        x_est = x_est + K*y;
        x_est(3) = wrap_angle(x_est(3));

        I = eye(3);
        P = (I - K*H)*P*(I - K*H)' + K*R*K';
        P = 0.5*(P + P');

        ex = x_est(1) - x_true(1);
        ey = x_est(2) - x_true(2);
        eth = wrap_angle(x_est(3) - x_true(3));
        sumPos2 = sumPos2 + ex^2 + ey^2;
        sumTh2  = sumTh2 + eth^2;
    end

    rmsPos = sqrt(sumPos2 / N);
    rmsTh  = sqrt(sumTh2  / N);

    fprintf('RMS position error [m]: %.4f\n', rmsPos);
    fprintf('RMS heading error [rad]: %.4f\n', rmsTh);
end

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

function xnext = motion_model(x, u, dt)
    px = x(1); py = x(2); th = x(3);
    v = u(1);  w  = u(2);
    xnext = zeros(3,1);
    xnext(1) = px + dt*v*cos(th);
    xnext(2) = py + dt*v*sin(th);
    xnext(3) = wrap_angle(th + dt*w);
end

function F = jacobian_F(x, u, dt)
    th = x(3);
    v  = u(1);
    F = eye(3);
    F(1,3) = -dt*v*sin(th);
    F(2,3) =  dt*v*cos(th);
end

function L = jacobian_L(x, dt)
    th = x(3);
    L = zeros(3,2);
    L(1,1) = dt*cos(th);
    L(2,1) = dt*sin(th);
    L(3,2) = dt;
end

function z = meas_model(x, lm)
    px = x(1); py = x(2); th = x(3);
    mx = lm(1); my = lm(2);
    dx = mx - px;
    dy = my - py;
    r  = sqrt(dx^2 + dy^2);
    b  = wrap_angle(atan2(dy, dx) - th);
    z = [r; b];
end

function H = jacobian_H(x, lm)
    px = x(1); py = x(2);
    mx = lm(1); my = lm(2);
    dx = mx - px;
    dy = my - py;
    q  = dx^2 + dy^2;
    eps = 1e-12;
    if q < eps, q = eps; end
    r  = sqrt(q);

    H = zeros(2,3);
    % range
    H(1,1) = -dx/r;
    H(1,2) = -dy/r;
    % bearing
    H(2,1) =  dy/q;
    H(2,2) = -dx/q;
    H(2,3) = -1;
end

function idx = nearest_landmark(x, landmarks)
    dx = landmarks(:,1) - x(1);
    dy = landmarks(:,2) - x(2);
    d  = sqrt(dx.^2 + dy.^2);
    [~, idx] = min(d);
end

Simulink mapping (no images): implement the EKF as two MATLAB Function blocks: (1) Predict, triggered by odometry sample time; (2) Update, triggered by measurement arrival. Maintain \( \hat{\mathbf{x}} \) and \( \mathbf{P} \) as persistent states in the block, with angle wrapping after each write to \( \theta \). If you have Robotics System Toolbox, you can connect sensor models and coordinate transforms using frame-transform utilities, but keep the estimator math inside explicit blocks for traceability.

File: Chapter7_Lesson1.nb

(* Chapter7_Lesson1.nb
   Chapter 7 - Lesson 1: EKF Localization Pipeline (mobile framing)

   Wolfram Language script saved with .nb extension for convenience.
   It symbolically derives Jacobians for the planar EKF motion and
   range-bearing measurement model, then performs one EKF step numerically.
*)

ClearAll["Global`*"];

wrapAngle[a_] := Mod[a + Pi, 2 Pi] - Pi;

(* State and control symbols *)
x = {px, py, th};
u = {v, w};
dt = Symbol["dt"];

(* Motion model f(x,u) *)
f[x_, u_] := {
  x[[1]] + dt u[[1]] Cos[x[[3]]],
  x[[2]] + dt u[[1]] Sin[x[[3]]],
  wrapAngle[x[[3]] + dt u[[2]]]
};

(* Jacobian F = df/dx using unwrapped theta for differentiation *)
fRaw = {
  px + dt v Cos[th],
  py + dt v Sin[th],
  th + dt w
};
F = D[fRaw, {{px, py, th}}] // Simplify;

(* Process noise enters via control noise n_v, n_w added to u *)
L = D[fRaw /. {v -> v + nv, w -> w + nw}, {{nv, nw}}] // Simplify;

(* Landmark and measurement model h(x) *)
lm = {mx, my};
h[x_, lm_] := Module[{dx, dy, r, b},
  dx = lm[[1]] - x[[1]];
  dy = lm[[2]] - x[[2]];
  r = Sqrt[dx^2 + dy^2];
  b = wrapAngle[ArcTan[dx, dy] - x[[3]]];
  {r, b}
];

hRaw = {Sqrt[(mx - px)^2 + (my - py)^2], ArcTan[mx - px, my - py] - th};
H = D[hRaw, {{px, py, th}}] // Simplify;

Print["F(x,u) = "]; Print[F];
Print["L(x) = "]; Print[L];
Print["H(x,lm) = "]; Print[H];

(* One numeric EKF step (single landmark measurement) *)
x0 = {0.0, 0.0, 0.0};
P0 = DiagonalMatrix[{0.5^2, 0.5^2, (10 Degree)^2}];

dt0 = 0.1;
u0 = {0.9, 0.25};

Q0 = DiagonalMatrix[{0.08^2, 0.05^2}];
R0 = DiagonalMatrix[{0.15^2, (2 Degree)^2}];

lm0 = {5.0, 0.0};

xPred = f[x0, u0] /. dt -> dt0 // N;
FP = F /. {px -> x0[[1]], py -> x0[[2]], th -> x0[[3]], v -> u0[[1]], w -> u0[[2]], dt -> dt0} // N;
LP = L /. {th -> x0[[3]], dt -> dt0} // N;

PPred = FP.P0.Transpose[FP] + LP.Q0.Transpose[LP] // N;

zTrue = h[xPred, lm0] // N;
zMeas = zTrue + {RandomVariate[NormalDistribution[0, Sqrt[R0[[1,1]]]]],
                 RandomVariate[NormalDistribution[0, Sqrt[R0[[2,2]]]]]};
zMeas[[2]] = wrapAngle[zMeas[[2]]];

hPred = h[xPred, lm0] // N;
y = zMeas - hPred; y[[2]] = wrapAngle[y[[2]]];

HP = H /. {px -> xPred[[1]], py -> xPred[[2]], th -> xPred[[3]], mx -> lm0[[1]], my -> lm0[[2]]} // N;
S = HP.PPred.Transpose[HP] + R0 // N;
K = PPred.Transpose[HP].Inverse[S] // N;

xUp = xPred + K.y; xUp[[3]] = wrapAngle[xUp[[3]]];

I3 = IdentityMatrix[3];
PUp = (I3 - K.HP).PPred.Transpose[(I3 - K.HP)] + K.R0.Transpose[K] // N;

Print["xPred = ", xPred];
Print["xUp   = ", xUp];
Print["PUp   = "]; Print[PUp];

Problem 1 (Motion Jacobian): For the planar motion 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 \), derive \( \mathbf{F}_k = \partial f/\partial \mathbf{x} \).

Solution: Write \( f = [f_1,f_2,f_3]^\top \). Then:

\[ \frac{\partial f_1}{\partial x} = 1,\quad \frac{\partial f_1}{\partial y} = 0,\quad \frac{\partial f_1}{\partial \theta} = -\Delta t\, v_k \sin\theta_k \]

\[ \frac{\partial f_2}{\partial x} = 0,\quad \frac{\partial f_2}{\partial y} = 1,\quad \frac{\partial f_2}{\partial \theta} = \Delta t\, v_k \cos\theta_k \]

\[ \frac{\partial f_3}{\partial x} = 0,\quad \frac{\partial f_3}{\partial y} = 0,\quad \frac{\partial f_3}{\partial \theta} = 1 \]

Therefore:

\[ \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}. \]

Problem 2 (Range–Bearing Jacobian): For a landmark \( \mathbf{m}=[m_x,m_y]^\top \), define \( d_x = m_x-x \), \( d_y = m_y-y \), \( q=d_x^2+d_y^2 \), \( r=\sqrt{q} \), and \( \phi = \mathrm{atan2}(d_y,d_x)-\theta \). Show that: \( \frac{\partial r}{\partial x} = -d_x/r,\; \frac{\partial r}{\partial y} = -d_y/r,\; \frac{\partial \phi}{\partial x} = d_y/q,\; \frac{\partial \phi}{\partial y} = -d_x/q,\; \frac{\partial \phi}{\partial \theta} = -1 \).

Solution: Use chain rule. Since \( r=\sqrt{q} \), we have \( \partial r/\partial x = \tfrac{1}{2}q^{-1/2}\cdot \partial q/\partial x \). But \( q=(m_x-x)^2+(m_y-y)^2 \), hence \( \partial q/\partial x = -2(m_x-x)=-2d_x \). Therefore \( \partial r/\partial x = \tfrac{1}{2}(1/r)(-2d_x)=-d_x/r \); similarly \( \partial r/\partial y=-d_y/r \). For the bearing, the standard planar derivative of \( \mathrm{atan2}(d_y,d_x) \) yields: \( \partial \phi/\partial x = \partial \mathrm{atan2}(d_y,d_x)/\partial x = d_y/q \), \( \partial \phi/\partial y = -d_x/q \), and subtracting \( \theta \) gives \( \partial \phi/\partial \theta = -1 \).

Problem 3 (Joseph Form Preserves PSD): Suppose \( \mathbf{P}_{k|k-1} \succeq 0 \) and \( \mathbf{R}_k \succeq 0 \). Prove that the Joseph covariance update \( \mathbf{P}_{k|k} = (\mathbf{I}-\mathbf{K}\mathbf{H})\mathbf{P}_{k|k-1}(\mathbf{I}-\mathbf{K}\mathbf{H})^\top + \mathbf{K}\mathbf{R}\mathbf{K}^\top \) satisfies \( \mathbf{P}_{k|k} \succeq 0 \).

Solution:

Let \( \mathbf{A} = (\mathbf{I}-\mathbf{K}\mathbf{H}) \). Then: \( \mathbf{A}\mathbf{P}_{k|k-1}\mathbf{A}^\top \succeq 0 \) because for any vector \( \mathbf{t} \), \( \mathbf{t}^\top \mathbf{A}\mathbf{P}\mathbf{A}^\top \mathbf{t} = (\mathbf{A}^\top \mathbf{t})^\top \mathbf{P} (\mathbf{A}^\top \mathbf{t}) \ge 0 \). Similarly \( \mathbf{K}\mathbf{R}\mathbf{K}^\top \succeq 0 \) because it is a congruence transform of a PSD matrix. The sum of PSD matrices is PSD, hence \( \mathbf{P}_{k|k} \succeq 0 \).

Problem 4 (Sequential vs Batch Update in Linear Case): Consider a linear measurement with stacked form \( \mathbf{z} = \begin{bmatrix}\mathbf{z}_1 \\ \mathbf{z}_2\end{bmatrix} \), \( \mathbf{H} = \begin{bmatrix}\mathbf{H}_1 \\ \mathbf{H}_2\end{bmatrix} \), and block-diagonal noise covariance \( \mathbf{R} = \mathrm{diag}(\mathbf{R}_1,\mathbf{R}_2) \). Show that applying two sequential KF updates (first with \( \mathbf{z}_1 \), then \( \mathbf{z}_2 \)) yields the same posterior mean and covariance as one batch update with stacked \( (\mathbf{z},\mathbf{H},\mathbf{R}) \).

Solution (information-form proof):

Define information matrix \( \mathbf{\Lambda} = \mathbf{P}^{-1} \) and information vector \( \boldsymbol{\eta} = \mathbf{P}^{-1}\hat{\mathbf{x}} \). A linear Gaussian update adds information:

\[ \mathbf{\Lambda}^+ \; ← \; \mathbf{\Lambda}^- + \mathbf{H}^\top \mathbf{R}^{-1}\mathbf{H}, \quad \boldsymbol{\eta}^+ \; ← \; \boldsymbol{\eta}^- + \mathbf{H}^\top \mathbf{R}^{-1}\mathbf{z}. \]

With two independent measurements, sequential updates add \( \mathbf{H}_1^\top \mathbf{R}_1^{-1}\mathbf{H}_1 \) and \( \mathbf{H}_2^\top \mathbf{R}_2^{-1}\mathbf{H}_2 \) in either order, and likewise for \( \boldsymbol{\eta} \). Addition commutes, so the final \( \mathbf{\Lambda}^+ \) and \( \boldsymbol{\eta}^+ \) match the batch update. Recover \( \mathbf{P}^+ = (\mathbf{\Lambda}^+)^{-1} \) and \( \hat{\mathbf{x}}^+ = \mathbf{P}^+ \boldsymbol{\eta}^+ \).

Problem 5 (1D Steady-State Riccati): Consider a 1D random walk: \( x_{k+1} = x_k + w_k \) with \( w_k \sim \mathcal{N}(0,q) \), and measurement \( z_k = x_k + v_k \) with \( v_k \sim \mathcal{N}(0,r) \). Find the steady-state posterior variance \( P^\star \) satisfying the Riccati fixed point.

Solution:

In 1D, prediction gives \( P^- = P + q \). Kalman gain is \( K = P^-/(P^- + r) \). Posterior variance is \( P^+ = (1-K)P^- = \frac{rP^-}{P^-+r} \). At steady state \( P^\star = \frac{r(P^\star+q)}{(P^\star+q)+r} \). Multiply both sides by \( (P^\star+q)+r \):

\[ P^\star(P^\star+q+r) \;=\; r(P^\star+q) \;\Rightarrow\; (P^\star)^2 + qP^\star + rP^\star \;=\; rP^\star + rq \;\Rightarrow\; (P^\star)^2 + qP^\star - rq = 0. \]

Solve the quadratic and keep the nonnegative root:

\[ P^\star \; ← \; \frac{-q + \sqrt{q^2 + 4rq}}{2}. \]

You assembled an EKF localization pipeline in a mobile-robot framing: a world-frame pose state updated by body-frame controls. We derived EKF prediction and correction as first-order linearization followed by Kalman algebra, computed Jacobians for a planar velocity model and a landmark range–bearing sensor, and implemented the full loop in Python, C++, Java, MATLAB, and Mathematica. This establishes the baseline filter-based localization method used broadly in AMR systems.

In the next lesson, you will contrast EKF linearization with sigma-point propagation and analyze when nonlinearities warrant a different approximation.