Visual odometry (VO) provides drift-limited constraints from images, but it is sensitive to lighting, motion blur, texture scarcity, and scale ambiguity (monocular). IMU integration provides high-rate motion information and observability of gravity-aligned attitude and metric scale, but it drifts due to bias and noise. VIO fuses both: inertial propagation acts as a fast prior, while vision periodically corrects drift by adding geometric constraints.

In AMR settings, we also care about: (i) long traversals with intermittent features, (ii) vibrations, (iii) planar motion priors, and (iv) wheel slip (which motivates multi-sensor redundancy). The fusion pipeline must therefore be robust, computationally bounded, and explicitly uncertainty-aware.

flowchart TD
  S["Sensors: camera + IMU (+ wheel, GPS)"] --> SYNC["Time alignment and calibration"]
  SYNC --> FE["Front-end: visual tracking and feature management"]
  SYNC --> IMU["IMU integration / preintegration"]
  FE --> LC["Loose coupling: VO pose -> filter/optimizer"]
  FE --> TC["Tight coupling: reprojection residuals"]
  IMU --> LC
  IMU --> TC
  LC --> EST["State estimator: EKF / Sliding-window"]
  TC --> EST
  EST --> OUT["Outputs: pose, velocity, biases, covariance"]
        

Let the latent state at time \( k \) be \( \mathbf{x}_k \) (pose, velocity, IMU biases). IMU provides high-rate data \( \mathbf{u}_k \), and vision provides lower-rate observations \( \mathbf{z}_k \). The generic Markov factorization used by both EKF and factor graphs is:

\[ p(\mathbf{x}_{0:K}\mid \mathbf{z}_{1:K},\mathbf{u}_{1:K}) \propto p(\mathbf{x}_0)\prod_{k=1}^K p(\mathbf{x}_k\mid \mathbf{x}_{k-1},\mathbf{u}_k)\; \prod_{k=1}^K p(\mathbf{z}_k\mid \mathbf{x}_k). \]

Under Gaussian assumptions for the process and measurement models, maximizing the posterior is equivalent to minimizing a weighted nonlinear least-squares objective (MAP ↔ least squares).

\[ \hat{\mathbf{x}}_{0:K} = \arg\min_{\mathbf{x}_{0:K}} \Big( \|\mathbf{r}_0(\mathbf{x}_0)\|_{\mathbf{P}_0^{-1}}^2 +\sum_{k=1}^K \|\mathbf{r}^{\text{imu}}_k(\mathbf{x}_{k-1},\mathbf{x}_k)\|_{\mathbf{Q}_k^{-1}}^2 +\sum_{k=1}^K \|\mathbf{r}^{\text{vis}}_k(\mathbf{x}_k)\|_{\mathbf{R}_k^{-1}}^2 \Big), \quad \|\mathbf{r}\|_{\mathbf{W}}^2 \triangleq \mathbf{r}^\top \mathbf{W}\mathbf{r}. \]

Proof sketch (MAP ↔ NLLS): If \( \mathbf{r} \sim \mathcal{N}(\mathbf{0},\mathbf{\Sigma}) \), then \( p(\mathbf{r}) \propto \exp\!\left(-\tfrac{1}{2}\mathbf{r}^\top\mathbf{\Sigma}^{-1}\mathbf{r}\right) \). Multiplying independent likelihood terms and taking \( -\log \) yields a sum of quadratic forms; minimizing that sum is MAP.

The IMU reports specific force and angular velocity in the IMU/body frame. A standard model is: \( \mathbf{f}_m \) (accelerometer) and \( \boldsymbol{\omega}_m \) (gyro).

\[ \mathbf{f}_m = \mathbf{R}^\top(\mathbf{a}-\mathbf{g}) + \mathbf{b}_f + \mathbf{n}_f, \qquad \boldsymbol{\omega}_m = \boldsymbol{\omega} + \mathbf{b}_\omega + \mathbf{n}_\omega, \]

where \( \mathbf{R} \) is the world-to-body rotation, \( \mathbf{g} \) is gravity, biases \( \mathbf{b}_f,\mathbf{b}_\omega \) are slowly varying, and noises are modeled as white. In discrete time, a pedagogical propagation (sufficient for AMR planar analysis) is:

\[ \begin{aligned} \boldsymbol{\theta}_{k+1} &= \boldsymbol{\theta}_k + (\boldsymbol{\omega}_{m,k}-\mathbf{b}_{\omega,k})\Delta t, \\ \mathbf{v}_{k+1} &= \mathbf{v}_k + \left(\mathbf{R}_k(\mathbf{f}_{m,k}-\mathbf{b}_{f,k}) + \mathbf{g}\right)\Delta t, \\ \mathbf{p}_{k+1} &= \mathbf{p}_k + \mathbf{v}_k\Delta t + \tfrac{1}{2}\left(\mathbf{R}_k(\mathbf{f}_{m,k}-\mathbf{b}_{f,k}) + \mathbf{g}\right)\Delta t^2. \end{aligned} \]

To propagate uncertainty, linearize around the current estimate using an error-state model \( \delta\mathbf{x}_k \):

\[ \delta\mathbf{x}_{k+1} = \mathbf{F}_k\delta\mathbf{x}_k + \mathbf{G}_k\mathbf{w}_k, \qquad \mathbf{P}_{k+1} = \mathbf{F}_k\mathbf{P}_k\mathbf{F}_k^\top + \mathbf{G}_k\mathbf{Q}_k\mathbf{G}_k^\top. \]

Practical note: In real 3D VIO, rotations are handled on a manifold (e.g., SO(3) with incremental perturbations) to avoid singularities. For AMR planar motion, a yaw angle perturbation is often sufficient and simplifies Jacobians, as used in the provided code.

Vision can be used as (A) a pose constraint (output of VO) or (B) raw pixel constraints (reprojection errors). These lead to two coupling modes:

• Loose coupling: the visual front-end produces a pose (or relative pose) with covariance; the back-end fuses it as a measurement.
• Tight coupling: the back-end directly minimizes reprojection residuals of tracked features jointly with inertial constraints, typically in a sliding window.

A common tight-coupled residual for a feature \( i \) observed at keyframe \( k \) is: \( \mathbf{r}(i,k) \):

\[ \mathbf{r}(i,k) = \mathbf{u}_{i,k} - \pi\!\left(\mathbf{T}_{CI}\mathbf{T}_{IW,k}^{-1}\mathbf{P}_i\right), \]

where \( \mathbf{u}_{i,k} \) is the measured pixel, \( \pi(\cdot) \) is the camera projection, \( \mathbf{T}_{CI} \) is the camera←IMU extrinsic, and \( \mathbf{T}_{IW,k} \) is the IMU pose in world. Robust penalties (Huber/Cauchy) are typically used to reduce sensitivity to outliers:

\[ \min_{\mathbf{x}} \sum_{i,k} \rho\!\left(\|\mathbf{r}(i,k)\|_{\mathbf{\Sigma}_{i,k}^{-1}}^2\right) + \sum_k \|\mathbf{r}^{\text{imu}}_k\|_{\mathbf{Q}_k^{-1}}^2, \quad \rho(\cdot)\ \text{robust loss}. \]

Sliding-window optimization would be too expensive if we integrated every IMU sample as a separate factor. IMU preintegration compresses many IMU samples between keyframes \( i \) and \( j \) into a small set of relative motion quantities: \( \Delta\mathbf{R}_{ij}, \Delta\mathbf{v}_{ij}, \Delta\mathbf{p}_{ij} \) and an associated covariance.

In a simplified discrete form (ignoring higher-order bias corrections for readability), define \( \Delta t \) as the time from \( i \) to \( j \):

\[ \begin{aligned} \Delta\mathbf{R}_{ij} ≈ \prod_{k=i}^{j-1} \exp\!\left((\boldsymbol{\omega}_{m,k}-\mathbf{b}_\omega)\Delta t_k\right), \\ \Delta\mathbf{v}_{ij} ≈ \sum_{k=i}^{j-1} \Delta\mathbf{R}_{ik}(\mathbf{f}_{m,k}-\mathbf{b}_f)\Delta t_k, \\ \Delta\mathbf{p}_{ij} ≈ \sum_{k=i}^{j-1}\left(\Delta\mathbf{v}_{ik}\Delta t_k + \tfrac{1}{2}\Delta\mathbf{R}_{ik}(\mathbf{f}_{m,k}-\mathbf{b}_f)\Delta t_k^2\right). \end{aligned} \]

The resulting IMU factor residuals compare these preintegrated quantities with the state variables at keyframes \( i \) and \( j \):

\[ \begin{aligned} \mathbf{r}_R &= \operatorname{Log}\!\left((\Delta\mathbf{R}_{ij})^{-1}\mathbf{R}_i^\top\mathbf{R}_j\right), \\ \mathbf{r}_v &= \mathbf{R}_i^\top(\mathbf{v}_j-\mathbf{v}_i-\mathbf{g}\Delta t) - \Delta\mathbf{v}_{ij}, \\ \mathbf{r}_p &= \mathbf{R}_i^\top(\mathbf{p}_j-\mathbf{p}_i-\mathbf{v}_i\Delta t - \tfrac{1}{2}\mathbf{g}\Delta t^2) - \Delta\mathbf{p}_{ij}. \end{aligned} \]

Key idea: preintegration is computed in the local frame of keyframe \( i \), so it does not depend on the global pose. Bias updates can be handled by first-order corrections that avoid recomputing the full preintegration for small bias changes.

In a filter-based pipeline, the IMU drives the prediction step at high rate, and visual information arrives as measurement updates. For an EKF, the measurement model is: \( \mathbf{z}_k = h(\mathbf{x}_k) + \mathbf{n}_k \). With linearization at \( \hat{\mathbf{x}}_k \):

\[ \mathbf{y}_k = \mathbf{z}_k - h(\hat{\mathbf{x}}_k), \qquad \mathbf{S}_k = \mathbf{H}_k\mathbf{P}_k\mathbf{H}_k^\top + \mathbf{R}_k, \qquad \mathbf{K}_k = \mathbf{P}_k\mathbf{H}_k^\top \mathbf{S}_k^{-1}. \]

\[ \hat{\mathbf{x}}_k^+ = \hat{\mathbf{x}}_k + \mathbf{K}_k\mathbf{y}_k, \qquad \mathbf{P}_k^+ = (\mathbf{I}-\mathbf{K}_k\mathbf{H}_k)\mathbf{P}_k(\mathbf{I}-\mathbf{K}_k\mathbf{H}_k)^\top + \mathbf{K}_k\mathbf{R}_k\mathbf{K}_k^\top. \]

The covariance expression above is the Joseph form. It preserves positive semidefiniteness under finite-precision arithmetic. Why: it is a sum of two PSD matrices: \( (\mathbf{I}-\mathbf{K}\mathbf{H})\mathbf{P}(\mathbf{I}-\mathbf{K}\mathbf{H})^\top \) and \( \mathbf{K}\mathbf{R}\mathbf{K}^\top \).

MSCKF-style insight: Instead of explicitly estimating every 3D landmark, one can keep a short history of camera poses and eliminate landmarks analytically using multi-view constraints. This yields measurement updates that constrain the trajectory without map states, making the filter efficient.

A sliding-window estimator maintains a window of recent keyframes and solves a nonlinear least-squares problem: \( \min \sum \|\text{residual}\|^2 \). Compared to EKF, it can be more accurate under strong nonlinearity because it relinearizes iteratively and can handle feature structure explicitly. The trade-off is higher computation.

A standard Gauss–Newton iteration solves:

\[ (\mathbf{J}^\top\mathbf{W}\mathbf{J})\delta = -\mathbf{J}^\top\mathbf{W}\mathbf{r}, \qquad \mathbf{x} \leftarrow \mathbf{x} \oplus \delta, \]

where \( \mathbf{J} \) is the Jacobian of stacked residuals, \( \mathbf{W} \) is the block weight matrix (information), and \( \oplus \) denotes a state update with appropriate rotation handling. The IMU part uses the preintegration residuals in Section 5; the visual part uses reprojection residuals in Section 4.

flowchart TD
  K["Select keyframes in window"] --> P["Build factors: IMU + visual + priors"]
  P --> L["Linearize residuals at current estimate"]
  L --> H["Assemble normal equations: J^T W J and J^T W r"]
  H --> S["Solve for delta (Cholesky or PCG)"]
  S --> U["Update state and relinearize"]
  U --> C["Converged?"]
  C -->|yes| M["Marginalize old states -> prior factor"]
  C -->|no| L
  M --> O["Output pose, velocity, biases"]
        

For VIO, some directions are fundamentally unobservable depending on sensing:

• Global position: adding a constant translation to all poses does not change relative visual constraints.
• Global yaw (around gravity): without a magnetometer or map constraints, yaw is often weakly/partially unobservable.
• Scale: monocular vision alone cannot recover metric scale; IMU acceleration and gravity provide scale observability when excitation exists.

A practical consistency test for EKF updates uses the Normalized Innovation Squared (NIS): \( \text{NIS}_k \):

\[ \text{NIS}_k = \mathbf{y}_k^\top \mathbf{S}_k^{-1}\mathbf{y}_k. \]

If the model is correct, \( \text{NIS}_k \) is approximately chi-square distributed with degrees of freedom equal to the measurement dimension. A gating rule rejects outliers when \( \text{NIS}_k > \chi^2_{d,\,\alpha} \).

AMR note: On ground robots, limited vertical excitation can make scale convergence slower; wheel odometry or known-height constraints can help stabilize estimation in low-excitation regimes.

Real VIO performance depends critically on calibration: camera intrinsics, IMU noise/bias parameters, and camera←IMU extrinsics \( \mathbf{T}_{CI} \). A simple time-offset model introduces a scalar offset \( \tau \) such that the visual timestamp \( t^c \) corresponds to IMU time \( t^i = t^c + \tau \). Estimators can either (i) pre-synchronize with hardware triggering, or (ii) estimate \( \tau \) online.

Online time-offset estimation is typically handled by augmenting the state and linearizing the measurement w.r.t. \( \tau \):

\[ \mathbf{z}(t^c) ≈ h(\mathbf{x}(t^c+\tau)) ≈ h(\mathbf{x}(t^c)) + \mathbf{H}_x\delta\mathbf{x} + \mathbf{H}_\tau \delta\tau. \]

The derivative \( \mathbf{H}_\tau \) can be approximated using the time derivative of the predicted measurement, computed from IMU propagation.

The following minimal labs implement a planar VIO fusion baseline: IMU propagation at \( 100\text{ Hz} \)-like rate and visual pose updates at a lower rate. This matches the conceptual pipeline of a loose-coupled VIO. For tight coupling, you would replace the visual pose update with reprojection residuals inside an optimizer.

10.1 Python (EKF-style fusion)

File: Chapter13_Lesson3.py

# Chapter13_Lesson3.py
# Planar visual-inertial loose coupling demo (EKF-like) for AMR
# NOTE: This is a didactic baseline, not a production VIO.

import numpy as np

def wrap_angle(theta: float) -> float:
    """Wrap angle to (-pi, pi]."""
    return (theta + np.pi) % (2 * np.pi) - np.pi

def rot2d(theta: float) -> np.ndarray:
    """2D rotation matrix."""
    c, s = np.cos(theta), np.sin(theta)
    return np.array([[c, -s],[s, c]])

class VIOPlanarEKF:
    """
    State x = [px, py, yaw, vx, vy, bg, bax, bay]^T
    where bg gyro bias, ba accel bias (x,y only).
    """
    def __init__(self):
        self.x = np.zeros(8)
        self.P = np.eye(8) * 1e-3

        # Process noise (continuous-time-ish heuristics)
        self.sigma_g = 0.02      # rad/sqrt(s)
        self.sigma_a = 0.20      # m/s^2/sqrt(s)
        self.sigma_bg = 0.001    # rad/s^2/sqrt(s) (bias random walk)
        self.sigma_ba = 0.01     # m/s^3/sqrt(s)

        # Visual measurement noise (pose)
        self.R_vis = np.diag([0.05**2, 0.05**2, (2*np.pi/180)**2])  # (m,m,rad)

        # Gravity (planar: no vertical, but keep concept)
        self.g = np.array([0.0, 0.0])

    def propagate_imu(self, omega_m, a_m, dt):
        """
        omega_m: measured yaw rate (rad/s)
        a_m: measured planar accel in body frame (ax, ay)
        """
        px, py, yaw, vx, vy, bg, bax, bay = self.x

        # Bias-corrected measurements
        omega = omega_m - bg
        a_body = np.array([a_m[0] - bax, a_m[1] - bay])

        # State propagation (simple Euler)
        yaw_new = wrap_angle(yaw + omega * dt)
        R = rot2d(yaw)  # body->world for accel
        a_world = R @ a_body + self.g

        vx_new = vx + a_world[0] * dt
        vy_new = vy + a_world[1] * dt
        px_new = px + vx * dt + 0.5 * a_world[0] * dt * dt
        py_new = py + vy * dt + 0.5 * a_world[1] * dt * dt

        # Bias random walk (no deterministic drift)
        self.x = np.array([px_new, py_new, yaw_new, vx_new, vy_new, bg, bax, bay])

        # Linearized covariance propagation: P = F P F^T + G Q G^T
        # For compactness, we use a simplified Jacobian capturing main couplings.
        F = np.eye(8)
        # d pos / d vel
        F[0,3] = dt
        F[1,4] = dt

        # yaw depends on bg
        F[2,5] = -dt

        # accel world depends on yaw and ba
        c, s = np.cos(yaw), np.sin(yaw)
        ax, ay = a_body
        # a_world = [c*ax - s*ay, s*ax + c*ay]
        dawx_dyaw = -s*ax - c*ay
        dawy_dyaw =  c*ax - s*ay
        # vel depends on yaw
        F[3,2] = dawx_dyaw * dt
        F[4,2] = dawy_dyaw * dt
        # vel depends on bax, bay (negative through a_body)
        # a_world wrt bax = -R[:,0], wrt bay = -R[:,1]
        F[3,6] = -c * dt
        F[3,7] =  s * dt
        F[4,6] = -s * dt
        F[4,7] = -c * dt

        # pos depends on yaw and ba as well (through 0.5 a dt^2)
        F[0,2] = 0.5 * dawx_dyaw * dt * dt
        F[1,2] = 0.5 * dawy_dyaw * dt * dt
        F[0,6] = -0.5 * c * dt * dt
        F[0,7] =  0.5 * s * dt * dt
        F[1,6] = -0.5 * s * dt * dt
        F[1,7] = -0.5 * c * dt * dt

        # Process noise: omega noise, accel noise, bias RW
        # Noise vector w = [n_g, n_ax, n_ay, n_bg, n_bax, n_bay]
        G = np.zeros((8,6))
        # yaw affected by gyro noise
        G[2,0] = dt
        # vel affected by accel noise (world frame approx using current R)
        G[3,1] = c * dt
        G[3,2] = -s * dt
        G[4,1] = s * dt
        G[4,2] =  c * dt
        # pos affected by accel noise (0.5 dt^2)
        G[0,1] = 0.5 * c * dt * dt
        G[0,2] = -0.5 * s * dt * dt
        G[1,1] = 0.5 * s * dt * dt
        G[1,2] = 0.5 * c * dt * dt

        # bias random walks
        G[5,3] = dt
        G[6,4] = dt
        G[7,5] = dt

        Q = np.diag([
            self.sigma_g**2,
            self.sigma_a**2,
            self.sigma_a**2,
            self.sigma_bg**2,
            self.sigma_ba**2,
            self.sigma_ba**2
        ])

        self.P = F @ self.P @ F.T + G @ Q @ G.T

    def update_visual_pose(self, z_pose):
        """
        z_pose = [px, py, yaw] from visual odometry (relative integrated to world frame).
        """
        # Measurement model: h(x) = [px, py, yaw]
        H = np.zeros((3,8))
        H[0,0] = 1.0
        H[1,1] = 1.0
        H[2,2] = 1.0

        z = np.array(z_pose)
        h = self.x[[0,1,2]]
        y = z - h
        y[2] = wrap_angle(y[2])

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

        # Joseph-form covariance update
        I = np.eye(8)
        self.x = self.x + K @ y
        self.x[2] = wrap_angle(self.x[2])
        self.P = (I - K @ H) @ self.P @ (I - K @ H).T + K @ self.R_vis @ K.T

def demo():
    ekf = VIOPlanarEKF()
    dt = 0.01
    T = 5.0
    N = int(T/dt)

    # Simulated motion: constant forward accel then steady
    omega_true = 0.05
    a_body_true = np.array([0.2, 0.0])

    # Biases
    bg = 0.01
    ba = np.array([0.05, -0.02])

    # Generate occasional visual pose measurements (10 Hz)
    vis_dt = 0.1
    vis_times = set([int(k*vis_dt/dt) for k in range(int(T/vis_dt)+1)])

    x_true = np.zeros(5)  # px,py,yaw,vx,vy
    traj = []

    for k in range(N):
        # True propagation
        yaw = x_true[2]
        R = rot2d(yaw)
        a_world = R @ a_body_true
        x_true[3] += a_world[0]*dt
        x_true[4] += a_world[1]*dt
        x_true[0] += x_true[3]*dt + 0.5*a_world[0]*dt*dt
        x_true[1] += x_true[4]*dt + 0.5*a_world[1]*dt*dt
        x_true[2] = wrap_angle(x_true[2] + omega_true*dt)

        # IMU measurements
        omega_m = omega_true + bg + np.random.normal(0, ekf.sigma_g/np.sqrt(dt))
        a_m = a_body_true + ba + np.random.normal(0, ekf.sigma_a/np.sqrt(dt), size=2)

        ekf.propagate_imu(omega_m, a_m, dt)

        # Visual update occasionally
        if k in vis_times:
            z_pose = np.array([x_true[0], x_true[1], x_true[2]]) + np.random.multivariate_normal(
                mean=np.zeros(3),
                cov=ekf.R_vis
            )
            ekf.update_visual_pose(z_pose)

        traj.append((k*dt, x_true.copy(), ekf.x.copy()))

    # Print final states
    t, xt, xe = traj[-1]
    print("Final time:", t)
    print("True [px,py,yaw,vx,vy]:", xt)
    print("EKF  [px,py,yaw,vx,vy]:", xe[:5])

if __name__ == "__main__":
    demo()
      

10.2 C++ (Eigen)

File: Chapter13_Lesson3.cpp

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

static double wrapAngle(double theta) {
  while (theta > M_PI) theta -= 2.0 * M_PI;
  while (theta <= -M_PI) theta += 2.0 * M_PI;
  return theta;
}

static Eigen::Matrix2d rot2d(double yaw) {
  double c = std::cos(yaw), s = std::sin(yaw);
  Eigen::Matrix2d R;
  R << c, -s, s, c;
  return R;
}

struct VIOPlanarEKF {
  // x = [px, py, yaw, vx, vy, bg, bax, bay]
  Eigen::VectorXd x;
  Eigen::MatrixXd P;

  double sigma_g = 0.02;
  double sigma_a = 0.20;
  double sigma_bg = 0.001;
  double sigma_ba = 0.01;

  Eigen::Matrix3d R_vis;

  VIOPlanarEKF() {
    x = Eigen::VectorXd::Zero(8);
    P = Eigen::MatrixXd::Identity(8,8) * 1e-3;
    R_vis.setZero();
    R_vis(0,0) = 0.05*0.05;
    R_vis(1,1) = 0.05*0.05;
    R_vis(2,2) = std::pow(2.0*M_PI/180.0, 2);
  }

  void propagateIMU(double omega_m, const Eigen::Vector2d& a_m, double dt) {
    double px = x(0), py = x(1), yaw = x(2), vx = x(3), vy = x(4);
    double bg = x(5), bax = x(6), bay = x(7);

    double omega = omega_m - bg;
    Eigen::Vector2d a_body(a_m(0) - bax, a_m(1) - bay);

    double yaw_new = wrapAngle(yaw + omega * dt);
    Eigen::Matrix2d R = rot2d(yaw);
    Eigen::Vector2d a_world = R * a_body; // planar (no gravity term)

    double vx_new = vx + a_world(0) * dt;
    double vy_new = vy + a_world(1) * dt;
    double px_new = px + vx * dt + 0.5 * a_world(0) * dt * dt;
    double py_new = py + vy * dt + 0.5 * a_world(1) * dt * dt;

    x << px_new, py_new, yaw_new, vx_new, vy_new, bg, bax, bay;

    Eigen::MatrixXd F = Eigen::MatrixXd::Identity(8,8);
    F(0,3) = dt;
    F(1,4) = dt;
    F(2,5) = -dt;

    double c = std::cos(yaw), s = std::sin(yaw);
    double ax = a_body(0), ay = a_body(1);
    double dawx_dyaw = -s*ax - c*ay;
    double dawy_dyaw =  c*ax - s*ay;

    F(3,2) = dawx_dyaw * dt;
    F(4,2) = dawy_dyaw * dt;

    // wrt biases: a_world = R*(a_m - b_a)
    F(3,6) = -c * dt;
    F(3,7) =  s * dt;
    F(4,6) = -s * dt;
    F(4,7) = -c * dt;

    F(0,2) = 0.5 * dawx_dyaw * dt * dt;
    F(1,2) = 0.5 * dawy_dyaw * dt * dt;

    F(0,6) = -0.5 * c * dt * dt;
    F(0,7) =  0.5 * s * dt * dt;
    F(1,6) = -0.5 * s * dt * dt;
    F(1,7) = -0.5 * c * dt * dt;

    Eigen::MatrixXd G = Eigen::MatrixXd::Zero(8,6);
    // w = [n_g, n_ax, n_ay, n_bg, n_bax, n_bay]
    G(2,0) = dt;
    G(3,1) = c * dt;
    G(3,2) = -s * dt;
    G(4,1) = s * dt;
    G(4,2) =  c * dt;

    G(0,1) = 0.5 * c * dt * dt;
    G(0,2) = -0.5 * s * dt * dt;
    G(1,1) = 0.5 * s * dt * dt;
    G(1,2) = 0.5 * c * dt * dt;

    G(5,3) = dt;
    G(6,4) = dt;
    G(7,5) = dt;

    Eigen::MatrixXd Q = Eigen::MatrixXd::Zero(6,6);
    Q(0,0) = sigma_g*sigma_g;
    Q(1,1) = sigma_a*sigma_a;
    Q(2,2) = sigma_a*sigma_a;
    Q(3,3) = sigma_bg*sigma_bg;
    Q(4,4) = sigma_ba*sigma_ba;
    Q(5,5) = sigma_ba*sigma_ba;

    P = F * P * F.transpose() + G * Q * G.transpose();
  }

  void updateVisualPose(const Eigen::Vector3d& z_pose) {
    Eigen::MatrixXd H = Eigen::MatrixXd::Zero(3,8);
    H(0,0) = 1.0;
    H(1,1) = 1.0;
    H(2,2) = 1.0;

    Eigen::Vector3d h(x(0), x(1), x(2));
    Eigen::Vector3d y = z_pose - h;
    y(2) = wrapAngle(y(2));

    Eigen::Matrix3d S = H * P * H.transpose() + R_vis;
    Eigen::MatrixXd K = P * H.transpose() * S.inverse();

    Eigen::MatrixXd I = Eigen::MatrixXd::Identity(8,8);
    x = x + K * y;
    x(2) = wrapAngle(x(2));
    P = (I - K*H) * P * (I - K*H).transpose() + K * R_vis * K.transpose();
  }
};

int main() {
  VIOPlanarEKF ekf;
  double dt = 0.01;
  double T = 5.0;
  int N = static_cast<int>(T/dt);

  double omega_true = 0.05;
  Eigen::Vector2d a_body_true(0.2, 0.0);

  double bg = 0.01;
  Eigen::Vector2d ba(0.05, -0.02);

  double vis_dt = 0.1;
  std::vector<int> vis_steps;
  for (int k = 0; k <= static_cast<int>(T/vis_dt); ++k) {
    vis_steps.push_back(static_cast<int>((k*vis_dt)/dt));
  }

  Eigen::VectorXd x_true = Eigen::VectorXd::Zero(5); // px,py,yaw,vx,vy

  std::mt19937 rng(0);
  std::normal_distribution<double> N01(0.0, 1.0);

  auto noise = [&](double sigma) { return sigma * N01(rng); };

  int vis_ptr = 0;
  for (int k = 0; k < N; ++k) {
    double yaw = x_true(2);
    Eigen::Matrix2d R = rot2d(yaw);
    Eigen::Vector2d a_world = R * a_body_true;

    x_true(3) += a_world(0) * dt;
    x_true(4) += a_world(1) * dt;
    x_true(0) += x_true(3) * dt + 0.5 * a_world(0) * dt * dt;
    x_true(1) += x_true(4) * dt + 0.5 * a_world(1) * dt * dt;
    x_true(2) = wrapAngle(x_true(2) + omega_true * dt);

    double omega_m = omega_true + bg + noise(ekf.sigma_g/std::sqrt(dt));
    Eigen::Vector2d a_m = a_body_true + ba;
    a_m(0) += noise(ekf.sigma_a/std::sqrt(dt));
    a_m(1) += noise(ekf.sigma_a/std::sqrt(dt));

    ekf.propagateIMU(omega_m, a_m, dt);

    if (vis_ptr < (int)vis_steps.size() && k == vis_steps[vis_ptr]) {
      vis_ptr++;
      Eigen::Vector3d z_pose(x_true(0), x_true(1), x_true(2));
      // add measurement noise
      z_pose(0) += noise(0.05);
      z_pose(1) += noise(0.05);
      z_pose(2) += noise(2.0*M_PI/180.0);
      z_pose(2) = wrapAngle(z_pose(2));

      ekf.updateVisualPose(z_pose);
    }
  }

  std::cout << "True: px=" << x_true(0) << " py=" << x_true(1)
            << " yaw=" << x_true(2) << " vx=" << x_true(3)
            << " vy=" << x_true(4) << "\n";
  std::cout << "EKF : px=" << ekf.x(0) << " py=" << ekf.x(1)
            << " yaw=" << ekf.x(2) << " vx=" << ekf.x(3)
            << " vy=" << ekf.x(4) << "\n";
  return 0;
}
      

10.3 Java (EJML)

File: Chapter13_Lesson3.java

import org.ejml.data.DMatrixRMaj;
import org.ejml.dense.row.CommonOps_DDRM;

import java.util.Random;

public class Chapter13_Lesson3 {

    static double wrapAngle(double theta) {
        while (theta > Math.PI) theta -= 2.0 * Math.PI;
        while (theta <= -Math.PI) theta += 2.0 * Math.PI;
        return theta;
    }

    static DMatrixRMaj rot2d(double yaw) {
        double c = Math.cos(yaw), s = Math.sin(yaw);
        DMatrixRMaj R = new DMatrixRMaj(2,2);
        R.set(0,0,c);  R.set(0,1,-s);
        R.set(1,0,s);  R.set(1,1, c);
        return R;
    }

    static class VIOPlanarEKF {
        // x = [px, py, yaw, vx, vy, bg, bax, bay]^T
        DMatrixRMaj x = new DMatrixRMaj(8,1);
        DMatrixRMaj P = CommonOps_DDRM.identity(8);

        double sigma_g = 0.02;
        double sigma_a = 0.20;
        double sigma_bg = 0.001;
        double sigma_ba = 0.01;

        DMatrixRMaj Rvis = new DMatrixRMaj(3,3);

        VIOPlanarEKF() {
            CommonOps_DDRM.scale(1e-3, P);
            CommonOps_DDRM.fill(x, 0.0);
            CommonOps_DDRM.fill(Rvis, 0.0);
            Rvis.set(0,0, 0.05*0.05);
            Rvis.set(1,1, 0.05*0.05);
            Rvis.set(2,2, Math.pow(2.0*Math.PI/180.0, 2));
        }

        void propagateIMU(double omega_m, double ax_m, double ay_m, double dt) {
            double px = x.get(0,0), py = x.get(1,0), yaw = x.get(2,0);
            double vx = x.get(3,0), vy = x.get(4,0);
            double bg = x.get(5,0), bax = x.get(6,0), bay = x.get(7,0);

            double omega = omega_m - bg;
            double axb = ax_m - bax;
            double ayb = ay_m - bay;

            double yawNew = wrapAngle(yaw + omega*dt);

            double c = Math.cos(yaw), s = Math.sin(yaw);
            // a_world = R(yaw) * a_body
            double awx = c*axb - s*ayb;
            double awy = s*axb + c*ayb;

            double vxNew = vx + awx*dt;
            double vyNew = vy + awy*dt;
            double pxNew = px + vx*dt + 0.5*awx*dt*dt;
            double pyNew = py + vy*dt + 0.5*awy*dt*dt;

            x.set(0,0, pxNew);
            x.set(1,0, pyNew);
            x.set(2,0, yawNew);
            x.set(3,0, vxNew);
            x.set(4,0, vyNew);

            // F
            DMatrixRMaj F = CommonOps_DDRM.identity(8);
            F.set(0,3, dt);
            F.set(1,4, dt);
            F.set(2,5, -dt);

            double dawx_dyaw = -s*axb - c*ayb;
            double dawy_dyaw =  c*axb - s*ayb;

            F.set(3,2, dawx_dyaw*dt);
            F.set(4,2, dawy_dyaw*dt);

            F.set(3,6, -c*dt);
            F.set(3,7,  s*dt);
            F.set(4,6, -s*dt);
            F.set(4,7, -c*dt);

            F.set(0,2, 0.5*dawx_dyaw*dt*dt);
            F.set(1,2, 0.5*dawy_dyaw*dt*dt);

            F.set(0,6, -0.5*c*dt*dt);
            F.set(0,7,  0.5*s*dt*dt);
            F.set(1,6, -0.5*s*dt*dt);
            F.set(1,7, -0.5*c*dt*dt);

            // G (8x6)
            DMatrixRMaj G = new DMatrixRMaj(8,6);
            // yaw noise
            G.set(2,0, dt);
            // accel noise (world approx)
            G.set(3,1, c*dt);   G.set(3,2, -s*dt);
            G.set(4,1, s*dt);   G.set(4,2,  c*dt);
            // pos accel noise
            G.set(0,1, 0.5*c*dt*dt);  G.set(0,2, -0.5*s*dt*dt);
            G.set(1,1, 0.5*s*dt*dt);  G.set(1,2,  0.5*c*dt*dt);
            // bias RW
            G.set(5,3, dt);
            G.set(6,4, dt);
            G.set(7,5, dt);

            DMatrixRMaj Q = new DMatrixRMaj(6,6);
            Q.set(0,0, sigma_g*sigma_g);
            Q.set(1,1, sigma_a*sigma_a);
            Q.set(2,2, sigma_a*sigma_a);
            Q.set(3,3, sigma_bg*sigma_bg);
            Q.set(4,4, sigma_ba*sigma_ba);
            Q.set(5,5, sigma_ba*sigma_ba);

            // P = F P F^T + G Q G^T
            DMatrixRMaj FP = new DMatrixRMaj(8,8);
            DMatrixRMaj FPFt = new DMatrixRMaj(8,8);
            CommonOps_DDRM.mult(F, P, FP);
            CommonOps_DDRM.multTransB(FP, F, FPFt);

            DMatrixRMaj GQ = new DMatrixRMaj(8,6);
            DMatrixRMaj GQGt = new DMatrixRMaj(8,8);
            CommonOps_DDRM.mult(G, Q, GQ);
            CommonOps_DDRM.multTransB(GQ, G, GQGt);

            CommonOps_DDRM.add(FPFt, GQGt, P);
        }

        void updateVisualPose(double zpx, double zpy, double zyaw) {
            // H maps [px,py,yaw]
            DMatrixRMaj H = new DMatrixRMaj(3,8);
            H.set(0,0, 1.0);
            H.set(1,1, 1.0);
            H.set(2,2, 1.0);

            DMatrixRMaj z = new DMatrixRMaj(3,1);
            z.set(0,0, zpx);
            z.set(1,0, zpy);
            z.set(2,0, zyaw);

            DMatrixRMaj h = new DMatrixRMaj(3,1);
            h.set(0,0, x.get(0,0));
            h.set(1,0, x.get(1,0));
            h.set(2,0, x.get(2,0));

            DMatrixRMaj y = new DMatrixRMaj(3,1);
            CommonOps_DDRM.subtract(z, h, y);
            y.set(2,0, wrapAngle(y.get(2,0)));

            DMatrixRMaj HP = new DMatrixRMaj(3,8);
            DMatrixRMaj S = new DMatrixRMaj(3,3);
            CommonOps_DDRM.mult(H, P, HP);
            CommonOps_DDRM.multTransB(HP, H, S);
            CommonOps_DDRM.addEquals(S, Rvis);

            DMatrixRMaj Sinv = new DMatrixRMaj(3,3);
            CommonOps_DDRM.invert(S, Sinv);

            DMatrixRMaj PHt = new DMatrixRMaj(8,3);
            CommonOps_DDRM.multTransB(P, H, PHt);

            DMatrixRMaj K = new DMatrixRMaj(8,3);
            CommonOps_DDRM.mult(PHt, Sinv, K);

            // x = x + K y
            DMatrixRMaj Ky = new DMatrixRMaj(8,1);
            CommonOps_DDRM.mult(K, y, Ky);
            CommonOps_DDRM.addEquals(x, Ky);
            x.set(2,0, wrapAngle(x.get(2,0)));

            // Joseph update: P = (I-KH) P (I-KH)^T + K R K^T
            DMatrixRMaj I = CommonOps_DDRM.identity(8);
            DMatrixRMaj KH = new DMatrixRMaj(8,8);
            CommonOps_DDRM.mult(K, H, KH);

            DMatrixRMaj IminusKH = new DMatrixRMaj(8,8);
            CommonOps_DDRM.subtract(I, KH, IminusKH);

            DMatrixRMaj tmp = new DMatrixRMaj(8,8);
            DMatrixRMaj term1 = new DMatrixRMaj(8,8);
            CommonOps_DDRM.mult(IminusKH, P, tmp);
            CommonOps_DDRM.multTransB(tmp, IminusKH, term1);

            DMatrixRMaj KR = new DMatrixRMaj(8,3);
            DMatrixRMaj term2 = new DMatrixRMaj(8,8);
            CommonOps_DDRM.mult(K, Rvis, KR);
            CommonOps_DDRM.multTransB(KR, K, term2);

            CommonOps_DDRM.add(term1, term2, P);
        }
    }

    public static void main(String[] args) {
        VIOPlanarEKF ekf = new VIOPlanarEKF();

        double dt = 0.01;
        double T = 5.0;
        int N = (int)(T/dt);

        double omegaTrue = 0.05;
        double axBodyTrue = 0.2;
        double ayBodyTrue = 0.0;

        double bg = 0.01;
        double bax = 0.05;
        double bay = -0.02;

        double visDt = 0.1;
        int[] visSteps = new int[(int)(T/visDt)+1];
        for(int i=0;i<visSteps.length;i++){
            visSteps[i] = (int)((i*visDt)/dt);
        }

        double px=0, py=0, yaw=0, vx=0, vy=0;

        Random rng = new Random(0);

        int visPtr = 0;
        for(int k=0;k<N;k++){
            double c = Math.cos(yaw), s = Math.sin(yaw);
            double awx = c*axBodyTrue - s*ayBodyTrue;
            double awy = s*axBodyTrue + c*ayBodyTrue;

            vx += awx*dt; vy += awy*dt;
            px += vx*dt + 0.5*awx*dt*dt;
            py += vy*dt + 0.5*awy*dt*dt;
            yaw = wrapAngle(yaw + omegaTrue*dt);

            double omega_m = omegaTrue + bg + rng.nextGaussian()*(ekf.sigma_g/Math.sqrt(dt));
            double ax_m = axBodyTrue + bax + rng.nextGaussian()*(ekf.sigma_a/Math.sqrt(dt));
            double ay_m = ayBodyTrue + bay + rng.nextGaussian()*(ekf.sigma_a/Math.sqrt(dt));

            ekf.propagateIMU(omega_m, ax_m, ay_m, dt);

            if (visPtr < visSteps.length && k == visSteps[visPtr]) {
                visPtr++;
                double zpx = px + rng.nextGaussian()*0.05;
                double zpy = py + rng.nextGaussian()*0.05;
                double zyaw = wrapAngle(yaw + rng.nextGaussian()*(2.0*Math.PI/180.0));
                ekf.updateVisualPose(zpx, zpy, zyaw);
            }
        }

        System.out.println("True: px=" + px + " py=" + py + " yaw=" + yaw + " vx=" + vx + " vy=" + vy);
        System.out.println("EKF : px=" + ekf.x.get(0,0) + " py=" + ekf.x.get(1,0) + " yaw=" + ekf.x.get(2,0)
                + " vx=" + ekf.x.get(3,0) + " vy=" + ekf.x.get(4,0));
    }
}
      

10.4 MATLAB / Simulink-Oriented Script

File: Chapter13_Lesson3.m

% Chapter13_Lesson3.m
% Planar VIO loose coupling demo (EKF-like) for AMR
% This script mirrors the Python/C++/Java baseline.

clear; clc;

wrapAngle = @(th) mod(th + pi, 2*pi) - pi;

rot2d = @(yaw) [cos(yaw), -sin(yaw); sin(yaw), cos(yaw)];

% State x = [px; py; yaw; vx; vy; bg; bax; bay]
x = zeros(8,1);
P = eye(8)*1e-3;

% Noise parameters
sigma_g  = 0.02;
sigma_a  = 0.20;
sigma_bg = 0.001;
sigma_ba = 0.01;

Rvis = diag([0.05^2, 0.05^2, (2*pi/180)^2]);

dt = 0.01;
T  = 5.0;
N  = floor(T/dt);

omega_true = 0.05;
a_body_true = [0.2; 0.0];

bg = 0.01;
ba = [0.05; -0.02];

vis_dt = 0.1;
vis_steps = round((0:vis_dt:T)/dt);
vis_ptr = 1;

x_true = zeros(5,1); % px,py,yaw,vx,vy

rng(0);

for k=1:N
    % true propagation
    yaw = x_true(3);
    R = rot2d(yaw);
    a_world = R * a_body_true;

    x_true(4) = x_true(4) + a_world(1)*dt;
    x_true(5) = x_true(5) + a_world(2)*dt;
    x_true(1) = x_true(1) + x_true(4)*dt + 0.5*a_world(1)*dt*dt;
    x_true(2) = x_true(2) + x_true(5)*dt + 0.5*a_world(2)*dt*dt;
    x_true(3) = wrapAngle(x_true(3) + omega_true*dt);

    % IMU measurements
    omega_m = omega_true + bg + randn()*(sigma_g/sqrt(dt));
    a_m = a_body_true + ba + randn(2,1)*(sigma_a/sqrt(dt));

    % propagate EKF
    px = x(1); py = x(2); yaw = x(3); vx = x(4); vy = x(5);
    bgk = x(6); bax = x(7); bay = x(8);

    omega = omega_m - bgk;
    a_body = [a_m(1)-bax; a_m(2)-bay];

    yaw_new = wrapAngle(yaw + omega*dt);
    R = rot2d(yaw);
    a_world = R * a_body; % planar

    vx_new = vx + a_world(1)*dt;
    vy_new = vy + a_world(2)*dt;
    px_new = px + vx*dt + 0.5*a_world(1)*dt*dt;
    py_new = py + vy*dt + 0.5*a_world(2)*dt*dt;

    x = [px_new; py_new; yaw_new; vx_new; vy_new; bgk; bax; bay];

    % Linearized F
    F = eye(8);
    F(1,4) = dt;
    F(2,5) = dt;
    F(3,6) = -dt;

    c = cos(yaw); s = sin(yaw);
    ax = a_body(1); ay = a_body(2);
    dawx_dyaw = -s*ax - c*ay;
    dawy_dyaw =  c*ax - s*ay;

    F(4,3) = dawx_dyaw*dt;
    F(5,3) = dawy_dyaw*dt;

    F(4,7) = -c*dt;
    F(4,8) =  s*dt;
    F(5,7) = -s*dt;
    F(5,8) = -c*dt;

    F(1,3) = 0.5*dawx_dyaw*dt*dt;
    F(2,3) = 0.5*dawy_dyaw*dt*dt;

    F(1,7) = -0.5*c*dt*dt;
    F(1,8) =  0.5*s*dt*dt;
    F(2,7) = -0.5*s*dt*dt;
    F(2,8) = -0.5*c*dt*dt;

    % G and Q
    G = zeros(8,6);
    G(3,1) = dt;
    G(4,2) = c*dt;  G(4,3) = -s*dt;
    G(5,2) = s*dt;  G(5,3) =  c*dt;

    G(1,2) = 0.5*c*dt*dt;  G(1,3) = -0.5*s*dt*dt;
    G(2,2) = 0.5*s*dt*dt;  G(2,3) = 0.5*c*dt*dt;

    G(6,4) = dt;
    G(7,5) = dt;
    G(8,6) = dt;

    Q = diag([sigma_g^2, sigma_a^2, sigma_a^2, sigma_bg^2, sigma_ba^2, sigma_ba^2]);

    P = F*P*F' + G*Q*G';

    % visual update
    if (vis_ptr <= length(vis_steps)) && (k == vis_steps(vis_ptr))
        vis_ptr = vis_ptr + 1;
        z = [x_true(1); x_true(2); x_true(3)] + chol(Rvis,'lower')*randn(3,1);

        H = zeros(3,8);
        H(1,1)=1; H(2,2)=1; H(3,3)=1;

        h = x(1:3);
        y = z - h;
        y(3) = wrapAngle(y(3));

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

        I = eye(8);
        x = x + K*y;
        x(3) = wrapAngle(x(3));
        P = (I-K*H)*P*(I-K*H)' + K*Rvis*K';
    end
end

disp('Final:');
disp(['True px py yaw vx vy: ', num2str(x_true')]);
disp(['EKF  px py yaw vx vy: ', num2str(x(1:5)')]);
      

10.5 Wolfram Mathematica (Symbolic Jacobian + EKF update demo)

File: Chapter13_Lesson3.nb

(* Chapter13_Lesson3.nb
   Mathematica/Wolfram Language content exported as a plain-text notebook-style script.
   Focus: symbolic Jacobians for a planar visual update and a minimal EKF update demo.
*)

ClearAll["Global`*"];

wrapAngle[th_] := Mod[th + Pi, 2 Pi] - Pi;

(* State x = {px, py, yaw, vx, vy, bg, bax, bay} *)
x = Array[x, 8];
{px, py, yaw, vx, vy, bg, bax, bay} = x;

(* Visual measurement model: z = h(x) + n, where h(x) = {px, py, yaw} *)
h[x_] := {x[[1]], x[[2]], x[[3]]};

H = D[h[x], {x}] // Simplify;
Print["H (Jacobian of visual measurement):"];
MatrixForm[H]

(* Joseph-form covariance update proof snippet: PSD-preserving structure *)
(* P+ = (I-KH) P (I-KH)^T + K R K^T *)

(* EKF update demo with numeric substitution *)
P = IdentityMatrix[8]*10^-3;
Rvis = DiagonalMatrix[{0.05^2, 0.05^2, (2 Pi/180)^2}];

z = {1.0, 0.5, 0.2};
xhat = ConstantArray[0.0, 8];
y = z - h[xhat];
y[[3]] = wrapAngle[y[[3]]];

S = H /. Thread[x -> xhat];
S = S . P . Transpose[S] + Rvis;

K = P . Transpose[H /. Thread[x -> xhat]] . Inverse[S];

xnew = xhat + K . y;
xnew[[3]] = wrapAngle[xnew[[3]]];

I8 = IdentityMatrix[8];
Pnew = (I8 - K.(H /. Thread[x -> xhat])) . P . 
        Transpose[I8 - K.(H /. Thread[x -> xhat])] + K . Rvis . Transpose[K];

Print["Updated state xnew:"];
xnew // N // MatrixForm

Print["Updated covariance diagonal:"];
Diagonal[Pnew] // N // MatrixForm
      

Suggested extension (for students): Replace the visual pose measurement with a feature reprojection update: estimate a relative pose from tracked features (e.g., PnP / essential matrix with RANSAC), then feed that constraint into the estimator with proper covariance and gating.

The following problems reinforce the mathematical and algorithmic foundations of visual–inertial fusion pipelines: loose vs tight coupling, EKF consistency, IMU preintegration, observability (scale/bias), and robust gating. Each problem includes a complete solution.

Problem 1 (MAP = Weighted Nonlinear Least Squares)

Suppose you have independent residual blocks \( \mathbf{r}_i(\mathbf{x}) \) with Gaussian noise \( \mathbf{r}_i \sim \mathcal{N}(\mathbf{0},\mathbf{\Sigma}_i) \), and a Gaussian prior on the state: \( \mathbf{x} \sim \mathcal{N}(\bar{\mathbf{x}}, \mathbf{P}_0) \). Show that the MAP estimate is equivalent (up to an additive constant) to minimizing a weighted nonlinear least squares cost.

Solution:

Independence implies the likelihood factorizes: \( p(\mathbf{z}\mid\mathbf{x}) = \prod_i p(\mathbf{r}_i(\mathbf{x})) \), where \( p(\mathbf{r}_i(\mathbf{x})) \propto \exp\!\left(-\tfrac{1}{2}\mathbf{r}_i(\mathbf{x})^\top\mathbf{\Sigma}_i^{-1}\mathbf{r}_i(\mathbf{x})\right) \). The prior is \( p(\mathbf{x}) \propto \exp\!\left(-\tfrac{1}{2}(\mathbf{x}-\bar{\mathbf{x}})^\top\mathbf{P}_0^{-1}(\mathbf{x}-\bar{\mathbf{x}})\right) \).

\[ \hat{\mathbf{x}}_{\text{MAP}} = \arg\max_{\mathbf{x}} p(\mathbf{x}\mid\mathbf{z}) = \arg\min_{\mathbf{x}}\Big( (\mathbf{x}-\bar{\mathbf{x}})^\top\mathbf{P}_0^{-1}(\mathbf{x}-\bar{\mathbf{x}}) + \sum_i \mathbf{r}_i(\mathbf{x})^\top\mathbf{\Sigma}_i^{-1}\mathbf{r}_i(\mathbf{x}) \Big), \]

since maximizing the posterior equals minimizing the negative log-posterior, and constant terms (not depending on \( \mathbf{x} \)) can be dropped. This is precisely a weighted nonlinear least squares objective with an added quadratic prior term.

Problem 2 (Joseph Form Preserves Positive Semidefiniteness)

In a loose-coupled EKF update, the covariance is updated via Joseph form:

\( \mathbf{P}^+ = (\mathbf{I}-\mathbf{K}\mathbf{H})\mathbf{P}^-(\mathbf{I}-\mathbf{K}\mathbf{H})^\top + \mathbf{K}\mathbf{R}\mathbf{K}^\top \).

Prove that if \( \mathbf{P}^- \succeq \mathbf{0} \) and \( \mathbf{R} \succeq \mathbf{0} \), then \( \mathbf{P}^+ \succeq \mathbf{0} \).

Solution:

Let \( \mathbf{A} = \mathbf{I}-\mathbf{K}\mathbf{H} \). For any vector \( \mathbf{v} \):

\[ \mathbf{v}^\top \mathbf{A}\mathbf{P}^-\mathbf{A}^\top \mathbf{v} = (\mathbf{A}^\top\mathbf{v})^\top \mathbf{P}^- (\mathbf{A}^\top\mathbf{v}) \ge 0 \quad (\text{since } \mathbf{P}^- \succeq \mathbf{0}). \]

Similarly:

\[ \mathbf{v}^\top \mathbf{K}\mathbf{R}\mathbf{K}^\top \mathbf{v} = (\mathbf{K}^\top\mathbf{v})^\top \mathbf{R} (\mathbf{K}^\top\mathbf{v}) \ge 0 \quad (\text{since } \mathbf{R} \succeq \mathbf{0}). \]

A sum of PSD matrices is PSD, hence \( \mathbf{P}^+ \succeq \mathbf{0} \).

Problem 3 (NIS Gating and Chi-Square Threshold)

Let the innovation be \( \mathbf{y}\in\mathbb{R}^d \) with covariance \( \mathbf{S} \). Under a correct model, assume \( \mathbf{y}\sim\mathcal{N}(\mathbf{0},\mathbf{S}) \). Define the Normalized Innovation Squared (NIS): \( \text{NIS} = \mathbf{y}^\top \mathbf{S}^{-1}\mathbf{y} \). Show that \( \text{NIS} \sim \chi^2_d \) and write the gating rule at significance \( \alpha \).

Solution:

Factor \( \mathbf{S}=\mathbf{L}\mathbf{L}^\top \) (Cholesky). Define \( \mathbf{w}=\mathbf{L}^{-1}\mathbf{y} \), so \( \mathbf{w}\sim\mathcal{N}(\mathbf{0},\mathbf{I}) \).

\[ \text{NIS}=\mathbf{y}^\top \mathbf{S}^{-1}\mathbf{y} = \mathbf{y}^\top (\mathbf{L}^{-\top}\mathbf{L}^{-1})\mathbf{y} = (\mathbf{L}^{-1}\mathbf{y})^\top(\mathbf{L}^{-1}\mathbf{y}) = \mathbf{w}^\top\mathbf{w} = \sum_{i=1}^d w_i^2 \sim \chi^2_d. \]

Gating at significance \( \alpha \) rejects an outlier measurement if \( \text{NIS} > \chi^2_{d,\,1-\alpha} \) and accepts if \( \text{NIS} \le \chi^2_{d,\,1-\alpha} \).

Problem 4 (IMU Preintegration Residuals for Velocity and Position)

Starting from discrete-time propagation with gravity:

\( \mathbf{v}_{k+1} = \mathbf{v}_k + (\mathbf{R}_k(\mathbf{f}_{m,k}-\mathbf{b}_a)+\mathbf{g})\Delta t \), \( \mathbf{p}_{k+1} = \mathbf{p}_k + \mathbf{v}_k\Delta t + \tfrac{1}{2}(\mathbf{R}_k(\mathbf{f}_{m,k}-\mathbf{b}_a)+\mathbf{g})\Delta t^2 \).

Derive the keyframe residuals: \( \mathbf{r}_v = \mathbf{R}_i^\top(\mathbf{v}_j-\mathbf{v}_i-\mathbf{g}\Delta t_{ij}) - \Delta\mathbf{v}_{ij} \) and \( \mathbf{r}_p = \mathbf{R}_i^\top(\mathbf{p}_j-\mathbf{p}_i-\mathbf{v}_i\Delta t_{ij}-\tfrac{1}{2}\mathbf{g}\Delta t_{ij}^2) - \Delta\mathbf{p}_{ij} \), where \( \Delta\mathbf{v}_{ij},\Delta\mathbf{p}_{ij} \) are the preintegrated increments in the \( i \)-frame.

Solution:

Summing the velocity updates from \( i \) to \( j \):

\[ \mathbf{v}_j-\mathbf{v}_i = \sum_{k=i}^{j-1}\left(\mathbf{R}_k(\mathbf{f}_{m,k}-\mathbf{b}_a)+\mathbf{g}\right)\Delta t_k. \]

Separate gravity and left-multiply by \( \mathbf{R}_i^\top \):

\[ \mathbf{R}_i^\top(\mathbf{v}_j-\mathbf{v}_i-\mathbf{g}\Delta t_{ij}) = \sum_{k=i}^{j-1}\mathbf{R}_i^\top\mathbf{R}_k(\mathbf{f}_{m,k}-\mathbf{b}_a)\Delta t_k. \]

Define the preintegrated velocity increment: \( \Delta\mathbf{v}_{ij} \triangleq \sum_{k=i}^{j-1}\mathbf{R}_i^\top\mathbf{R}_k(\mathbf{f}_{m,k}-\mathbf{b}_a)\Delta t_k \), yielding \( \mathbf{r}_v \).

For position, sum the position updates and substitute the velocity recursion to isolate gravity and the initial velocity term, then express the remaining specific-force contribution in the \( i \)-frame as \( \Delta\mathbf{p}_{ij} \). This gives:

\[ \mathbf{R}_i^\top(\mathbf{p}_j-\mathbf{p}_i-\mathbf{v}_i\Delta t_{ij}-\tfrac{1}{2}\mathbf{g}\Delta t_{ij}^2) = \Delta\mathbf{p}_{ij}, \]

and therefore the residual \( \mathbf{r}_p \) as stated.

Problem 5 (Scale Observability: Monocular Vision vs VIO)

(i) Explain why monocular visual odometry is scale-ambiguous. (ii) Show how IMU introduces metric information that makes scale observable under sufficiently rich motion. Use the specific force relation \( \mathbf{f} \approx \mathbf{R}^\top(\ddot{\mathbf{p}}-\mathbf{g}) \) in your argument.

Solution:

(i) Monocular reprojection constraints are invariant under a similarity transform: if you scale the 3D structure and camera translations by \( s \), pixel projections remain unchanged. Thus, the visual likelihood does not uniquely determine \( s \).

(ii) IMU measures metric acceleration (in SI units). If you scale the trajectory as \( \mathbf{p}'(t)=s\mathbf{p}(t) \), then \( \ddot{\mathbf{p}}'(t)=s\ddot{\mathbf{p}}(t) \), so

\[ \mathbf{f}' \approx \mathbf{R}^\top(\ddot{\mathbf{p}}' - \mathbf{g}) = \mathbf{R}^\top(s\ddot{\mathbf{p}} - \mathbf{g}), \]

which generally differs from \( \mathbf{f}\approx \mathbf{R}^\top(\ddot{\mathbf{p}}-\mathbf{g}) \) unless \( s=1 \) (or the motion has insufficient excitation so \( \ddot{\mathbf{p}} \) carries no informative content). Therefore, matching IMU measurements constrains scale in VIO given sufficiently exciting motion.

Problem 6 (Why Underestimating VO Covariance Breaks Loose Coupling)

In a loose-coupled EKF, a VO module outputs a pose measurement \( \mathbf{z}^{vo}_k \) with covariance \( \mathbf{R}^{vo}_k \). Explain how \( \mathbf{R}^{vo}_k \) affects the Kalman gain and why underestimating it leads to inconsistency and possible divergence, especially under outliers (motion blur, rolling shutter, time offset).

Solution:

The innovation covariance is \( \mathbf{S}_k = \mathbf{H}_k\mathbf{P}^-_k\mathbf{H}_k^\top + \mathbf{R}^{vo}_k \). The Kalman gain is \( \mathbf{K}_k = \mathbf{P}^-_k\mathbf{H}_k^\top \mathbf{S}_k^{-1} \). If \( \mathbf{R}^{vo}_k \) is underestimated, then \( \mathbf{S}_k \) is too small and \( \mathbf{K}_k \) becomes too large, so the filter over-trusts VO and pulls the state toward potentially corrupted measurements. The posterior covariance becomes overly optimistic (too small), so consistency tests (NIS/NEES) are violated and the filter may diverge when unmodeled effects create heavy-tailed residuals. Robust gating and conservative covariance modeling reduce this risk.

We derived a unified probabilistic view of VIO pipelines, showing how high-rate IMU propagation and lower-rate visual constraints form either an EKF-style filter or a sliding-window optimizer. We introduced IMU preintegration to build efficient inertial factors, discussed robust coupling strategies and gating, and highlighted AMR-specific issues such as limited excitation and calibration/time alignment sensitivity.