In a pose graph, each edge encodes a relative measurement between two poses. Standard optimization assumes near-Gaussian noise. Outliers violate this assumption: their errors have heavy tails or are multi-modal, so a sum-of-squared residuals objective can be dominated by a few wrong constraints.

Let node \( i \) have pose \( \mathbf{x}_i \), and edge \( (i,j) \) provide measurement \( \mathbf{z}_{ij} \). The edge defines a residual \( \mathbf{r}_{ij}(\mathbf{x}_i,\mathbf{x}_j) \) which should be near zero for a correct estimate. A single false loop closure can “pull” the whole solution away from the trajectory supported by the majority of edges.

Robust kernels fix this by reducing the influence of large residuals without brittle hard rejection (which can fail early in optimization when residuals are temporarily large).

For each edge, define the squared Mahalanobis error \( s_{ij} \):

\[ s_{ij}(\mathbf{x}) = \mathbf{r}_{ij}(\mathbf{x})^{\top}\,\mathbf{\Omega}_{ij}\,\mathbf{r}_{ij}(\mathbf{x}), \qquad \mathbf{\Omega}_{ij}=\mathbf{\Sigma}_{ij}^{-1}. \]

The classical objective minimizes \( F(\mathbf{x})=\sum_{(i,j)\in\mathcal{E}} s_{ij}(\mathbf{x}) \). Linearizing at \( \mathbf{x}^{(k)} \) yields \( \mathbf{r}_{ij}(\mathbf{x}^{(k)}\oplus\Delta\mathbf{x}) \approx \mathbf{r}_{ij} + \mathbf{J}_{ij}\Delta\mathbf{x} \), leading to sparse normal equations \( \mathbf{H}\Delta\mathbf{x}=-\mathbf{g} \) (Gauss–Newton) or damped (LM).

The weakness: \( s_{ij} \) grows quadratically with residual magnitude, so an outlier can dominate both \( \mathbf{H} \) and \( \mathbf{g} \).

Robust estimation replaces the quadratic penalty on \( s \) by a sub-quadratic function \( \rho(s) \) (a robust kernel):

\[ \min_{\mathbf{x}}\;F_{\rho}(\mathbf{x}) \;=\;\sum_{(i,j)\in\mathcal{E}} \rho\!\left(s_{ij}(\mathbf{x})\right). \]

3.1 Huber kernel (convex “soft clipping”)

A common convention applies the kernel to \( \sqrt{s}=\|\mathbf{r}\|_{\Omega} \):

\[ \rho_{\text{Huber}}(s)= \begin{cases} s, & \sqrt{s}\le \delta \\ 2\delta\sqrt{s}-\delta^2, & \sqrt{s} \gt \delta \end{cases} \]

For small errors it matches least squares; beyond \( \delta \) it grows linearly in \( \sqrt{s} \).

3.2 Redescending kernels (aggressive suppression)

Kernels like Tukey’s biweight can have redescending influence: extremely large residuals receive near-zero weight. This is powerful but introduces non-convexity and additional local minima. In mobile-robot SLAM, Huber is often a safe first choice.

For a scalar residual \( r \) with penalty \( \phi(r) \), define the influence function \( \psi(r)=\frac{d}{dr}\phi(r) \). Least squares uses \( \phi(r)=r^2 \), hence \( \psi(r)=2r \) (unbounded as \( |r|\to\infty \)).

Robust kernels bound or slow the growth of \( \psi(r) \). For the scalar Huber penalty:

\[ \phi_{\text{Huber}}(r)= \begin{cases} r^2, & |r|\le \delta \\ 2\delta|r|-\delta^2, & |r| \gt \delta \end{cases} \quad\Longrightarrow\quad \psi_{\text{Huber}}(r)= \begin{cases} 2r, & |r|\le \delta \\ 2\delta\,\operatorname{sign}(r), & |r| \gt \delta. \end{cases} \]

Thus a single huge outlier cannot create arbitrarily large force on the optimizer—precisely what we need when false loop closures exist.

Robust kernels correspond to heavy-tailed noise models. Assume independent edges with likelihood \( p(\mathbf{z}_{ij}\mid\mathbf{x})\propto \exp\!\big(-\tfrac{1}{2}\rho(s_{ij}(\mathbf{x}))\big) \). Then the MAP estimate is:

\[ \mathbf{x}_{\text{MAP}} =\arg\max_{\mathbf{x}}\prod_{(i,j)\in\mathcal{E}}p(\mathbf{z}_{ij}\mid\mathbf{x}) =\arg\min_{\mathbf{x}}\sum_{(i,j)\in\mathcal{E}}\rho\!\left(s_{ij}(\mathbf{x})\right). \]

Example (Cauchy-style). If a scalar residual has density \( p(r)\propto\frac{1}{1+(r/c)^2} \), then the negative log-likelihood is \( \phi(r)=\log\!\left(1+(r/c)^2\right) \), which grows only logarithmically for large \( |r| \).

We minimize \( \sum_i \rho(s_i) \), where \( s_i=\mathbf{r}_i^{\top}\Omega_i\mathbf{r}_i \). Linearize \( \mathbf{r}_i(\mathbf{x}^{(k)}\oplus\Delta\mathbf{x})\approx \mathbf{r}_i+\mathbf{J}_i\Delta\mathbf{x} \).

6.1 IRLS surrogate and weight

A standard IRLS step freezes a scalar weight computed from current residuals and solves:

\[ \Delta\mathbf{x}^{(k)} =\arg\min_{\Delta\mathbf{x}} \sum_i w_i\,\left\|\mathbf{r}_i+\mathbf{J}_i\Delta\mathbf{x}\right\|_{\Omega_i}^2, \qquad w_i=\rho'(s_i^{(k)}). \]

This reuses the same sparse solver as least squares, but with scaled information \( \Omega_i \leftarrow w_i\Omega_i \).

6.2 Huber weight (explicit)

\[ w(s)=\rho'(s)= \begin{cases} 1, & \sqrt{s}\le \delta \\ \delta/\sqrt{s}, & \sqrt{s} \gt \delta. \end{cases} \]

flowchart TD
  A["Inputs: current poses x, edges (i,j,z,Omega), kernel params"] --> B["For each edge: compute residual r and scalar s = r^T Omega r"]
  B --> C["Compute weight w from s (e.g., Huber w=1 or w=delta/sqrt(s))"]
  C --> D["Linearize: r(x + dx) approx r + J dx (numeric or analytic Jacobians)"]
  D --> E["Assemble normal equations with scaled info: Omega <- w*Omega"]
  E --> F["Solve (H + lambda I) dx = -g"]
  F --> G["Update poses; repeat until ||dx|| small"]
        

Stability note. Robust kernels can introduce non-convexity (especially redescending kernels). Practical strategy: start with a mild kernel (Huber) and use damping (LM) or continuation (start with large \( \delta \), decrease).

Common robust kernels in SLAM back-ends include Huber, Cauchy, Geman–McClure, and Tukey. Huber is convex and typically robust enough for occasional wrong loop closures; redescending kernels are stronger but more non-convex.

7.1 Scale selection using inlier statistics

Let residual dimension be \( d \) (e.g., \( d=3 \) for 2D relative pose). Under the correct Gaussian inlier model, \( s \) is approximately chi-square: \( s\sim\chi^2_d \). Choose \( \delta \) so that inliers stay in the quadratic region with probability \( p \):

\[ \mathbb{P}(s\le \delta^2)=p \quad\Longrightarrow\quad \delta=\sqrt{\chi^2_{d,p}}. \]

7.2 Library mapping (what you see in real SLAM stacks)

• g2o: per-edge robust kernel objects (Huber/Cauchy/etc.) that scale information matrices during optimization.
• Ceres Solver: LossFunction (HuberLoss, CauchyLoss, …) attached to residual blocks.
• GTSAM: robust noise models (m-estimators) wrapping Gaussian factors.

Robust kernels are a back-end defense layer: they do not repair perception, but they prevent a small number of bad constraints from corrupting the global estimate.

flowchart TD
  S["Sensors (LiDAR / vision / wheel / IMU)"] --> FE["Front-end: feature or scan matching"]
  FE --> ED["Create constraints: odometry edges + loop closures"]
  ED --> QC["Basic checks: time sync, covariance sanity, optional gating"]
  QC --> OPT["Back-end: nonlinear least squares with robust kernels"]
  OPT --> OUT["Output: optimized trajectory + quality metrics"]
        

The following implementation uses numeric Jacobians (finite differences) for clarity and includes one incorrect loop closure to illustrate robust downweighting. It solves twice: (i) plain least squares and (ii) Huber IRLS.

Rendering note: inside HTML <code> blocks, characters like < / > are escaped for safe rendering. The downloadable files contain executable code.

Chapter12_Lesson4.py

#!/usr/bin/env python3
# Chapter12_Lesson4.py
# Graph-Based SLAM (2D Pose Graph) with Robust Kernels via IRLS (Huber)
# Minimal from-scratch educational implementation (numeric Jacobians).

import numpy as np

def wrap_angle(a):
    return (a + np.pi) % (2.0 * np.pi) - np.pi

def between(xi, xj):
    """Relative pose z = xi^{-1} ∘ xj expressed in i-frame.
    xi, xj: [x, y, theta]
    """
    dx = xj[0] - xi[0]
    dy = xj[1] - xi[1]
    c = np.cos(xi[2]); s = np.sin(xi[2])
    xr =  c * dx + s * dy
    yr = -s * dx + c * dy
    tr = wrap_angle(xj[2] - xi[2])
    return np.array([xr, yr, tr], dtype=float)

def huber_weight_from_s(s, delta):
    """Huber on sqrt(s): rho(s)=s if sqrt(s)<=delta else 2*delta*sqrt(s)-delta^2.
    IRLS weight w = rho'(s) = 1 if sqrt(s)<=delta else delta/sqrt(s).
    """
    r = np.sqrt(max(s, 1e-12))
    return 1.0 if r <= delta else (delta / r)

def numeric_jacobian_edge(xi, xj, z, eps=1e-6):
    """Return Jacobians of error e wrt xi and xj (3x3 each) via finite differences."""
    def err(a, b):
        zhat = between(a, b)
        e = zhat - z
        e[2] = wrap_angle(e[2])
        return e

    e0 = err(xi, xj)
    Ji = np.zeros((3, 3))
    Jj = np.zeros((3, 3))

    for k in range(3):
        d = np.zeros(3); d[k] = eps
        ei = err(xi + d, xj)
        Ji[:, k] = (ei - e0) / eps

        ej = err(xi, xj + d)
        Jj[:, k] = (ej - e0) / eps

    return e0, Ji, Jj

def solve_pose_graph_irls(
    x_init, edges,
    robust="huber", delta=1.5,
    n_iters=15, damping=1e-6,
    fix_first=True
):
    """Gauss-Newton / IRLS for 2D pose graph.
    x_init: (N,3)
    edges: list of dicts {i,j,z(3,),Omega(3x3)}
    robust: "none" or "huber"
    """
    x = x_init.copy()
    N = x.shape[0]

    # Variable ordering: optionally fix pose 0 to remove gauge freedom
    if fix_first:
        var_nodes = list(range(1, N))
        base = 1
    else:
        var_nodes = list(range(N))
        base = 0

    dim = 3 * len(var_nodes)
    node_to_col = {nid: 3 * idx for idx, nid in enumerate(var_nodes)}

    for it in range(n_iters):
        H = np.zeros((dim, dim))
        b = np.zeros((dim,))

        total_rho = 0.0
        total_s = 0.0
        for e in edges:
            i, j = e["i"], e["j"]
            z = e["z"]
            Omega = e["Omega"]

            ei, Ji, Jj = numeric_jacobian_edge(x[i], x[j], z)
            s = float(ei.T @ Omega @ ei)
            total_s += s

            if robust == "huber":
                w = huber_weight_from_s(s, delta)
                # robust cost value (for monitoring)
                r = np.sqrt(max(s, 1e-12))
                if r <= delta:
                    rho = s
                else:
                    rho = 2.0 * delta * r - delta * delta
                total_rho += rho
            else:
                w = 1.0
                total_rho += s

            W = w * Omega

            # Assemble into normal equations, skipping fixed node
            def add_block(a, Ja, bnode, Jb):
                Ha = Ja.T @ W @ Ja
                Hab = Ja.T @ W @ Jb
                ga = Ja.T @ W @ ei

                ca = node_to_col.get(a, None)
                cb = node_to_col.get(bnode, None)
                if ca is not None:
                    H[ca:ca+3, ca:ca+3] += Ha
                    b[ca:ca+3] += ga
                if ca is not None and cb is not None:
                    H[ca:ca+3, cb:cb+3] += Hab
                # NOTE: symmetric part is added when edge is processed; we'll enforce symmetry later.

            # i block
            add_block(i, Ji, j, Jj)
            # j block
            add_block(j, Jj, i, Ji)  # adds Hj and Hji, gj

        # Symmetrize, add damping (Levenberg-style)
        H = 0.5 * (H + H.T)
        H += damping * np.eye(dim)

        # Solve for dx: H dx = -b
        try:
            dx = -np.linalg.solve(H, b)
        except np.linalg.LinAlgError:
            dx = -np.linalg.lstsq(H, b, rcond=None)[0]

        # Apply update
        for nid in var_nodes:
            c = node_to_col[nid]
            x[nid, 0] += dx[c + 0]
            x[nid, 1] += dx[c + 1]
            x[nid, 2] = wrap_angle(x[nid, 2] + dx[c + 2])

        print(f"iter {it:02d}  sum_s={total_s:.3f}  sum_rho={total_rho:.3f}  |dx|={np.linalg.norm(dx):.3e}")

        if np.linalg.norm(dx) < 1e-8:
            break

    return x

def make_synthetic_graph(N=20, sigma_xy=0.05, sigma_th=0.02, seed=0):
    rng = np.random.default_rng(seed)

    # Ground truth: a smooth arc
    gt = np.zeros((N, 3))
    for k in range(1, N):
        gt[k, 0] = gt[k-1, 0] + 0.3 * np.cos(gt[k-1, 2])
        gt[k, 1] = gt[k-1, 1] + 0.3 * np.sin(gt[k-1, 2])
        gt[k, 2] = wrap_angle(gt[k-1, 2] + 0.05)

    # Odometry edges (chain)
    Omega = np.diag([1.0/(sigma_xy**2), 1.0/(sigma_xy**2), 1.0/(sigma_th**2)])
    edges = []
    for k in range(N-1):
        z = between(gt[k], gt[k+1])
        z_noisy = z + rng.normal([0,0,0], [sigma_xy, sigma_xy, sigma_th])
        z_noisy[2] = wrap_angle(z_noisy[2])
        edges.append({"i": k, "j": k+1, "z": z_noisy, "Omega": Omega})

    # One correct loop closure (optional)
    z_loop = between(gt[3], gt[N-2]) + rng.normal([0,0,0], [sigma_xy, sigma_xy, sigma_th])
    z_loop[2] = wrap_angle(z_loop[2])
    edges.append({"i": 3, "j": N-2, "z": z_loop, "Omega": Omega})

    # One outlier loop closure
    z_bad = np.array([2.0, -1.0, 1.0])  # clearly wrong relative pose
    edges.append({"i": 0, "j": N-1, "z": z_bad, "Omega": Omega})

    # Initial guess: integrate odometry only
    x0 = np.zeros_like(gt)
    for k in range(1, N):
        z = edges[k-1]["z"]
        # compose: x_k = x_{k-1} ∘ z  (approx)
        th = x0[k-1, 2]
        c = np.cos(th); s = np.sin(th)
        x0[k, 0] = x0[k-1, 0] + c*z[0] - s*z[1]
        x0[k, 1] = x0[k-1, 1] + s*z[0] + c*z[1]
        x0[k, 2] = wrap_angle(x0[k-1, 2] + z[2])

    return gt, x0, edges

if __name__ == "__main__":
    gt, x0, edges = make_synthetic_graph(N=25, seed=1)

    print("\n=== Solve without robust kernel (sensitive to outlier) ===")
    x_ls = solve_pose_graph_irls(x0, edges, robust="none", n_iters=15, damping=1e-6)

    print("\n=== Solve with Huber robust kernel (IRLS) ===")
    x_huber = solve_pose_graph_irls(x0, edges, robust="huber", delta=1.5, n_iters=15, damping=1e-6)

    # Simple endpoint comparison
    print("\nEndpoint (pose N-1):")
    print("  ground truth:", gt[-1])
    print("  least squares:", x_ls[-1])
    print("  huber:", x_huber[-1])
      

In Ceres, robust kernels are implemented by attaching a LossFunction (e.g., HuberLoss) to each residual block. This example uses a compact 2D relative-pose constraint and includes one bad loop closure; the Huber loss suppresses its effect.

Chapter12_Lesson4.cpp

// Chapter12_Lesson4.cpp
// Robust pose-graph style constraints using Ceres Solver robust losses (Huber).
// This is a compact example illustrating how robust kernels appear in practice.
// Build (example):
//   mkdir build && cd build
//   cmake .. && cmake --build .
// Requires: Ceres Solver installed.

#include <ceres/ceres.h>
#include <cmath>
#include <iostream>
#include <vector>

static inline double WrapAngle(double a) {
  while (a > M_PI) a -= 2.0 * M_PI;
  while (a < -M_PI) a += 2.0 * M_PI;
  return a;
}

struct RelativePose2dCost {
  RelativePose2dCost(double dx, double dy, double dth, double sxy, double sth)
      : dx_(dx), dy_(dy), dth_(dth), sxy_(sxy), sth_(sth) {}

  template <typename T>
  bool operator()(const T* const pi, const T* const pj, T* residuals) const {
    // pi = [xi, yi, thi], pj = [xj, yj, thj]
    const T c = ceres::cos(pi[2]);
    const T s = ceres::sin(pi[2]);
    const T dxw = pj[0] - pi[0];
    const T dyw = pj[1] - pi[1];

    // relative translation in i-frame
    const T xrel =  c * dxw + s * dyw;
    const T yrel = -s * dxw + c * dyw;

    // relative angle
    const T threl = ceres::atan2(ceres::sin(pj[2] - pi[2]), ceres::cos(pj[2] - pi[2]));

    residuals[0] = (xrel - T(dx_)) / T(sxy_);
    residuals[1] = (yrel - T(dy_)) / T(sxy_);
    residuals[2] = (threl - T(dth_)) / T(sth_);
    return true;
  }

  double dx_, dy_, dth_;
  double sxy_, sth_;
};

int main() {
  // Tiny graph: chain + one bad loop closure to show robustness.
  const int N = 15;
  std::vector<double> poses(3 * N, 0.0);

  auto Pose = [&](int i) { return &poses[3 * i]; };

  // Initialize along x with small yaw
  for (int k = 1; k < N; ++k) {
    Pose(k)[0] = Pose(k - 1)[0] + 0.3;
    Pose(k)[1] = 0.0;
    Pose(k)[2] = 0.02 * k;
  }

  const double sigma_xy = 0.05;
  const double sigma_th = 0.02;

  ceres::Problem problem;

  // Add all parameter blocks
  for (int i = 0; i < N; ++i) {
    problem.AddParameterBlock(Pose(i), 3);
  }
  // Fix first pose (gauge)
  problem.SetParameterBlockConstant(Pose(0));

  // Odometry edges (near-perfect synthetic)
  for (int k = 0; k < N - 1; ++k) {
    const double dx = 0.3;
    const double dy = 0.0;
    const double dth = 0.02;

    ceres::CostFunction* cost =
        new ceres::AutoDiffCostFunction<RelativePose2dCost, 3, 3, 3>(
            new RelativePose2dCost(dx, dy, dth, sigma_xy, sigma_th));

    // Robust kernel: Huber loss
    ceres::LossFunction* loss = new ceres::HuberLoss(1.5);

    problem.AddResidualBlock(cost, loss, Pose(k), Pose(k + 1));
  }

  // One bad loop closure (outlier)
  {
    const int i = 0;
    const int j = N - 1;
    const double dx_bad = 2.0;
    const double dy_bad = -1.0;
    const double dth_bad = 1.0;

    ceres::CostFunction* cost =
        new ceres::AutoDiffCostFunction<RelativePose2dCost, 3, 3, 3>(
            new RelativePose2dCost(dx_bad, dy_bad, dth_bad, sigma_xy, sigma_th));

    // The same robust kernel will downweight this constraint as residual grows.
    ceres::LossFunction* loss = new ceres::HuberLoss(1.5);

    problem.AddResidualBlock(cost, loss, Pose(i), Pose(j));
  }

  ceres::Solver::Options options;
  options.linear_solver_type = ceres::DENSE_QR;
  options.max_num_iterations = 30;
  options.minimizer_progress_to_stdout = true;

  ceres::Solver::Summary summary;
  ceres::Solve(options, &problem, &summary);

  std::cout << "\nFinal pose of last node:\n";
  std::cout << "x=" << Pose(N - 1)[0] << " y=" << Pose(N - 1)[1]
            << " th=" << Pose(N - 1)[2] << "\n";

  return 0;
}
      

This Java implementation mirrors the Python solver: compute residuals, compute Huber weights, assemble normal equations, solve, and iterate. It fixes the first node to remove gauge freedom.

Chapter12_Lesson4.java

// Chapter12_Lesson4.java
// Graph-Based SLAM (2D pose graph) with Huber IRLS (numeric Jacobians) in pure Java.
// Educational: focuses on the robust weighting logic rather than performance.

import java.util.ArrayList;
import java.util.List;

public class Chapter12_Lesson4 {

    static class Edge {
        int i, j;
        double[] z;      // [dx, dy, dtheta] = pose_i^{-1} ∘ pose_j in i-frame
        double[][] Omega; // 3x3 information matrix
        Edge(int i, int j, double[] z, double[][] Omega) {
            this.i=i; this.j=j; this.z=z; this.Omega=Omega;
        }
    }

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

    static double[] between(double[] xi, double[] xj) {
        double dx = xj[0] - xi[0];
        double dy = xj[1] - xi[1];
        double c = Math.cos(xi[2]), s = Math.sin(xi[2]);
        double xr =  c * dx + s * dy;
        double yr = -s * dx + c * dy;
        double tr = wrap(xj[2] - xi[2]);
        return new double[]{xr, yr, tr};
    }

    static double huberWeightFromS(double s, double delta) {
        double r = Math.sqrt(Math.max(s, 1e-12));
        return (r <= delta) ? 1.0 : (delta / r);
    }

    static class EdgeLinearization {
        double[] e;     // 3
        double[][] Ji;  // 3x3
        double[][] Jj;  // 3x3
        EdgeLinearization(double[] e, double[][] Ji, double[][] Jj) {
            this.e=e; this.Ji=Ji; this.Jj=Jj;
        }
    }

    static EdgeLinearization numericJacobianEdge(double[] xi, double[] xj, double[] z, double eps) {
        double[] e0 = err(xi, xj, z);
        double[][] Ji = new double[3][3];
        double[][] Jj = new double[3][3];

        for (int k=0; k<3; k++) {
            double[] d = new double[]{0,0,0};
            d[k] = eps;

            double[] xi2 = new double[]{xi[0]+d[0], xi[1]+d[1], xi[2]+d[2]};
            double[] ei = err(xi2, xj, z);
            for (int r=0; r<3; r++) Ji[r][k] = (ei[r]-e0[r]) / eps;

            double[] xj2 = new double[]{xj[0]+d[0], xj[1]+d[1], xj[2]+d[2]};
            double[] ej = err(xi, xj2, z);
            for (int r=0; r<3; r++) Jj[r][k] = (ej[r]-e0[r]) / eps;
        }
        return new EdgeLinearization(e0, Ji, Jj);
    }

    static double[] err(double[] xi, double[] xj, double[] z) {
        double[] zhat = between(xi, xj);
        double[] e = new double[]{zhat[0]-z[0], zhat[1]-z[1], wrap(zhat[2]-z[2])};
        return e;
    }

    static double quadForm(double[] v, double[][] A) {
        double[] Av = new double[3];
        for (int i=0;i<3;i++){
            Av[i]=0;
            for(int j=0;j<3;j++) Av[i]+=A[i][j]*v[j];
        }
        double s=0;
        for(int i=0;i<3;i++) s+=v[i]*Av[i];
        return s;
    }

    static double[][] matMulT(double[][] J, double[][] A, double[][] K) {
        // return J^T A K ; here all are 3x3
        double[][] out = new double[3][3];
        for (int r=0;r<3;r++){
            for (int c=0;c<3;c++){
                double sum=0;
                for (int i=0;i<3;i++){
                    for (int j=0;j<3;j++){
                        sum += J[i][r] * A[i][j] * K[j][c];
                    }
                }
                out[r][c]=sum;
            }
        }
        return out;
    }

    static double[] matVecMulT(double[][] J, double[][] A, double[] e) {
        // return J^T A e
        double[] out = new double[3];
        for (int r=0;r<3;r++){
            double sum=0;
            for (int i=0;i<3;i++){
                for (int j=0;j<3;j++){
                    sum += J[i][r] * A[i][j] * e[j];
                }
            }
            out[r]=sum;
        }
        return out;
    }

    static void add3(double[][] H, int ra, int ca, double[][] blk) {
        for (int r=0;r<3;r++){
            for(int c=0;c<3;c++){
                H[ra+r][ca+c]+=blk[r][c];
            }
        }
    }
    static void add3v(double[] b, int ra, double[] v) {
        for(int r=0;r<3;r++) b[ra+r]+=v[r];
    }

    static double[] solveLinear(double[][] A, double[] b) {
        // naive Gaussian elimination (for small systems)
        int n = b.length;
        double[][] M = new double[n][n];
        double[] x = new double[n];
        double[] bb = new double[n];
        for(int i=0;i<n;i++){
            System.arraycopy(A[i], 0, M[i], 0, n);
            bb[i]=b[i];
        }
        // forward
        for(int k=0;k<n;k++){
            // pivot
            int piv=k;
            double best=Math.abs(M[k][k]);
            for(int i=k+1;i<n;i++){
                if(Math.abs(M[i][k])>best){best=Math.abs(M[i][k]);piv=i;}
            }
            if(piv!=k){
                double[] tmp=M[k]; M[k]=M[piv]; M[piv]=tmp;
                double t=bb[k]; bb[k]=bb[piv]; bb[piv]=t;
            }
            double diag=M[k][k];
            if(Math.abs(diag)<1e-12) continue;
            for(int i=k+1;i<n;i++){
                double f=M[i][k]/diag;
                for(int j=k;j<n;j++) M[i][j]-=f*M[k][j];
                bb[i]-=f*bb[k];
            }
        }
        // back substitution
        for(int i=n-1;i>=0;i--){
            double sum=bb[i];
            for(int j=i+1;j<n;j++) sum-=M[i][j]*x[j];
            double diag=M[i][i];
            x[i]=(Math.abs(diag)<1e-12)?0:(sum/diag);
        }
        return x;
    }

    static double[][] diagOmega(double sxy, double sth) {
        double[][] O = new double[3][3];
        O[0][0] = 1.0/(sxy*sxy);
        O[1][1] = 1.0/(sxy*sxy);
        O[2][2] = 1.0/(sth*sth);
        return O;
    }

    static double[][] scaleMat(double a, double[][] M) {
        double[][] out = new double[3][3];
        for(int i=0;i<3;i++) for(int j=0;j<3;j++) out[i][j]=a*M[i][j];
        return out;
    }

    public static void main(String[] args) {
        int N = 20;
        double[][] x = new double[N][3];

        // initial chain
        for (int k=1;k<N;k++){
            x[k][0] = x[k-1][0] + 0.3;
            x[k][1] = 0.0;
            x[k][2] = wrap(x[k-1][2] + 0.05);
        }

        double sigmaXY=0.05, sigmaTH=0.02;
        double[][] Omega = diagOmega(sigmaXY, sigmaTH);

        List<Edge> edges = new ArrayList<>();
        for(int k=0;k<N-1;k++){
            edges.add(new Edge(k, k+1, new double[]{0.3,0.0,0.05}, Omega));
        }
        // bad loop closure (outlier)
        edges.add(new Edge(0, N-1, new double[]{2.0,-1.0,1.0}, Omega));

        // Optimize with IRLS-Huber
        int iters=15;
        double delta=1.5;
        double eps=1e-6;
        double damping=1e-6;

        // Fix node 0 (remove gauge): variables are nodes 1..N-1
        int varN = N-1;
        int dim = 3*varN;

        for(int it=0; it<iters; it++){
            double[][] H = new double[dim][dim];
            double[] b = new double[dim];

            double sumS=0, sumRho=0;

            for(Edge ed : edges){
                int i=ed.i, j=ed.j;
                EdgeLinearization lin = numericJacobianEdge(x[i], x[j], ed.z, eps);
                double s = quadForm(lin.e, ed.Omega);
                sumS += s;

                double w = huberWeightFromS(s, delta);
                double r = Math.sqrt(Math.max(s, 1e-12));
                double rho = (r <= delta) ? s : (2.0*delta*r - delta*delta);
                sumRho += rho;

                double[][] W = scaleMat(w, ed.Omega);

                // blocks if variable (skip node 0)
                Integer ci = (i==0)? null : 3*(i-1);
                Integer cj = (j==0)? null : 3*(j-1);

                // Hii, Hij, gj
                if(ci != null){
                    double[][] Hii = matMulT(lin.Ji, W, lin.Ji);
                    double[] gi = matVecMulT(lin.Ji, W, lin.e);
                    add3(H, ci, ci, Hii);
                    add3v(b, ci, gi);
                }
                if(cj != null){
                    double[][] Hjj = matMulT(lin.Jj, W, lin.Jj);
                    double[] gj = matVecMulT(lin.Jj, W, lin.e);
                    add3(H, cj, cj, Hjj);
                    add3v(b, cj, gj);
                }
                if(ci != null && cj != null){
                    double[][] Hij = matMulT(lin.Ji, W, lin.Jj);
                    double[][] Hji = matMulT(lin.Jj, W, lin.Ji);
                    add3(H, ci, cj, Hij);
                    add3(H, cj, ci, Hji);
                }
            }

            // damping
            for(int d=0; d<dim; d++) H[d][d] += damping;

            // solve H dx = -b
            double[] rhs = new double[dim];
            for(int k=0;k<dim;k++) rhs[k] = -b[k];
            double[] dx = solveLinear(H, rhs);

            double norm=0;
            for(double v: dx) norm += v*v;
            norm = Math.sqrt(norm);

            // apply update
            for(int node=1; node<N; node++){
                int c = 3*(node-1);
                x[node][0] += dx[c+0];
                x[node][1] += dx[c+1];
                x[node][2] = wrap(x[node][2] + dx[c+2]);
            }

            System.out.printf("iter %02d  sumS=%.3f  sumRho=%.3f  |dx|=%.3e%n", it, sumS, sumRho, norm);
            if(norm < 1e-8) break;
        }

        System.out.println("Final pose of last node: x=" + x[N-1][0] + " y=" + x[N-1][1] + " th=" + x[N-1][2]);
    }
}
      

The MATLAB script below implements IRLS-Huber. For Simulink, place the IRLS step in a MATLAB Function block that outputs \( \Delta\mathbf{x} \), then integrate pose states with Unit Delay blocks inside a For-Iterator subsystem (see comments in code).

Chapter12_Lesson4.m

% Chapter12_Lesson4.m
% Graph-Based SLAM (2D pose graph) with robust kernels via IRLS (Huber).
% Educational MATLAB implementation using numeric Jacobians and Gauss-Newton.

function Chapter12_Lesson4()
    rng(1);
    N = 20;
    x = zeros(N,3);
    for k=2:N
        x(k,1) = x(k-1,1) + 0.3;
        x(k,2) = 0.0;
        x(k,3) = wrap(x(k-1,3) + 0.05);
    end

    sigmaXY = 0.05; sigmaTH = 0.02;
    Omega = diag([1/sigmaXY^2, 1/sigmaXY^2, 1/sigmaTH^2]);

    edges = {};
    for k=1:N-1
        e.i = k; e.j = k+1;
        e.z = [0.3; 0.0; 0.05];
        e.Omega = Omega;
        edges{end+1} = e;
    end
    % bad loop closure (outlier)
    e.i = 1; e.j = N; e.z = [2.0; -1.0; 1.0]; e.Omega = Omega;
    edges{end+1} = e;

    delta = 1.5;
    iters = 15;
    damping = 1e-6;

    % Fix node 1 => variables nodes 2..N
    dim = 3*(N-1);

    for it=1:iters
        H = zeros(dim,dim);
        b = zeros(dim,1);

        sumS = 0; sumRho = 0;

        for idx=1:numel(edges)
            ed = edges{idx};
            i = ed.i; j = ed.j;

            [eij, Ji, Jj] = numericJacobianEdge(x(i,:)' , x(j,:)' , ed.z, 1e-6);

            s = eij' * ed.Omega * eij;
            sumS = sumS + s;

            w = huberWeightFromS(s, delta);
            r = sqrt(max(s, 1e-12));
            if r <= delta
                rho = s;
            else
                rho = 2*delta*r - delta^2;
            end
            sumRho = sumRho + rho;

            W = w * ed.Omega;

            ci = nodeToCol(i, N);
            cj = nodeToCol(j, N);

            if ci > 0
                H(ci:ci+2, ci:ci+2) = H(ci:ci+2, ci:ci+2) + Ji' * W * Ji;
                b(ci:ci+2) = b(ci:ci+2) + Ji' * W * eij;
            end
            if cj > 0
                H(cj:cj+2, cj:cj+2) = H(cj:cj+2, cj:cj+2) + Jj' * W * Jj;
                b(cj:cj+2) = b(cj:cj+2) + Jj' * W * eij;
            end
            if (ci > 0) && (cj > 0)
                H(ci:ci+2, cj:cj+2) = H(ci:ci+2, cj:cj+2) + Ji' * W * Jj;
                H(cj:cj+2, ci:ci+2) = H(cj:cj+2, ci:ci+2) + Jj' * W * Ji;
            end
        end

        H = 0.5*(H+H') + damping*eye(dim);
        dx = -H \ b;

        % apply
        for node=2:N
            c = 3*(node-2) + 1;
            x(node,1) = x(node,1) + dx(c+0);
            x(node,2) = x(node,2) + dx(c+1);
            x(node,3) = wrap(x(node,3) + dx(c+2));
        end

        fprintf('iter %02d  sumS=%.3f  sumRho=%.3f  |dx|=%.3e\n', it, sumS, sumRho, norm(dx));
        if norm(dx) < 1e-8
            break;
        end
    end

    disp('Final pose of last node:');
    disp(x(end,:));

    % Simulink note:
    % - You can implement the IRLS loop with a MATLAB Function block that outputs dx,
    %   then integrate pose states with Unit Delay blocks, using a For-Iterator subsystem.
end

function c = nodeToCol(node, N)
    % node 1 fixed => return 0. Others map to 1-based column index.
    if node == 1
        c = 0;
    else
        c = 3*(node-2) + 1;
    end
end

function w = huberWeightFromS(s, delta)
    r = sqrt(max(s, 1e-12));
    if r <= delta
        w = 1.0;
    else
        w = delta / r;
    end
end

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

function z = between(xi, xj)
    dx = xj(1) - xi(1);
    dy = xj(2) - xi(2);
    c = cos(xi(3)); s = sin(xi(3));
    xr =  c*dx + s*dy;
    yr = -s*dx + c*dy;
    tr = wrap(xj(3) - xi(3));
    z = [xr; yr; tr];
end

function e = err(xi, xj, z)
    zhat = between(xi, xj);
    e = zhat - z;
    e(3) = wrap(e(3));
end

function [e0, Ji, Jj] = numericJacobianEdge(xi, xj, z, eps)
    e0 = err(xi, xj, z);
    Ji = zeros(3,3);
    Jj = zeros(3,3);
    for k=1:3
        d = zeros(3,1); d(k)=eps;
        ei = err(xi+d, xj, z);
        Ji(:,k) = (ei - e0)/eps;
        ej = err(xi, xj+d, z);
        Jj(:,k) = (ej - e0)/eps;
    end
end
      

This notebook expression derives the Huber weight \( w(s)=\rho'(s) \) and demonstrates IRLS on a 1D robust location problem.

Chapter12_Lesson4.nb

(* Chapter12_Lesson4.nb
   Wolfram Language notebook expression (plain-text).
   Focus: deriving robust weights and a small IRLS demo for 1D location with outliers. *)

Notebook[{
  Cell["Chapter 12 - Lesson 4: Handling Outliers (Robust Kernels)", "Title"],
  Cell["IRLS weight derivation for Huber kernel (rho applied to s = r^2).", "Section"],

  Cell[BoxData[
    RowBox[{
      RowBox[{"rho", "[", "s_", "]"}], ":=",
      RowBox[{"Piecewise", "[",
        RowBox[{
          RowBox[{"{",
            RowBox[{
              RowBox[{"{", RowBox[{"s", ",", RowBox[{"Sqrt", "[", "s", "]"}], "<=", "c"}], "}"}],
              ",",
              RowBox[{"{", RowBox[{
                RowBox[{
                  RowBox[{"2", " ", "c", " ", RowBox[{"Sqrt", "[", "s", "]"}]}], "-", RowBox[{"c", "^", "2"}]
                }],
                ",",
                RowBox[{"Sqrt", "[", "s", "]"}], ">", "c"
              }], "}"}]
            }], "}"}],
          ",", "s"
        }], "]"}]}]
  ], "Input"],

  Cell["Derivative and IRLS weight w = rho'(s).", "Text"],
  Cell[BoxData[
    RowBox[{
      RowBox[{"w", "[", "s_", "]"}], ":=",
      RowBox[{"D", "[",
        RowBox[{
          RowBox[{"rho", "[", "s", "]"}], ",", "s"
        }], "]"}]}]
  ], "Input"],

  Cell["Check piecewise form of w(s):", "Text"],
  Cell[BoxData[RowBox[{"FullSimplify", "[", RowBox[{"w", "[", "s", "]"}], "]"}]], "Input"],

  Cell["IRLS for 1D location: minimize Sum rho((x - yi)^2) over x.", "Section"],
  Cell[BoxData[
    RowBox[{
      RowBox[{"ys", "=", RowBox[{"Join", "[",
        RowBox[{
          RowBox[{"RandomVariate", "[", RowBox[{"NormalDistribution", "[", RowBox[{"0", ",", "1"}], "]"}], ",", "40"}], "]"}],
          ",",
          RowBox[{"{", RowBox[{"8", ",", "9", ",", "10"}], "}"}]
        }], "]"}]}], ";"}]
  ], "Input"],

  Cell[BoxData[RowBox[{"c", "=", "1.5"}]], "Input"],

  Cell[BoxData[
    RowBox[{
      RowBox[{"irls", "[", RowBox[{"x0_", ",", "iters_"}], "]"}], ":=",
      RowBox[{"Module", "[",
        RowBox[{
          RowBox[{"{", RowBox[{"x", "=", "x0", ",", "r", ",", "s", ",", "ws"}], "}"}],
          ",",
          RowBox[{"Do", "[",
            RowBox[{
              RowBox[{
                RowBox[{"r", "=", RowBox[{"x", "-", "ys"}]}], ";",
                RowBox[{"s", "=", RowBox[{"r", "^", "2"}]}], ";",
                RowBox[{"ws", "=", RowBox[{"w", " /@ ", "s"}]}], ";",
                RowBox[{"x", "=", RowBox[{"Total", "[", RowBox[{"ws", " ", "ys"}], "]"}], "/", RowBox[{"Total", "[", "ws", "]"}]}], ";"
              }],
              ",",
              RowBox[{"{", RowBox[{"iters"}], "}"}]
            }], "]"}], ";",
          "x"
        }], "]"}]}]
  ], "Input"],

  Cell[BoxData[RowBox[{"x_hat", "=", RowBox[{"irls", "[", RowBox[{"0", ",", "25"}], "]"}]}]], "Input"],
  Cell[BoxData[RowBox[{"{", RowBox[{"Mean", "[", "ys", "]"}], ",", "x_hat"}], "}"}]], "Input"],

  Cell["The robust estimate x_hat is less influenced by the extreme outliers than Mean[ys].", "Text"]
}]
      

Problem 1 (IRLS surrogate): Starting from \( \min_{\mathbf{x}}\sum_i \rho(s_i(\mathbf{x})) \) with \( s_i=\mathbf{r}_i^{\top}\Omega_i\mathbf{r}_i \), justify the IRLS surrogate \( \min_{\Delta\mathbf{x}}\sum_i w_i\|\mathbf{r}_i+\mathbf{J}_i\Delta\mathbf{x}\|_{\Omega_i}^2 \) with \( w_i=\rho'(s_i^{(k)}) \).

Solution: Let \( s_i(\Delta\mathbf{x})=(\mathbf{r}_i+\mathbf{J}_i\Delta\mathbf{x})^{\top}\Omega_i(\mathbf{r}_i+\mathbf{J}_i\Delta\mathbf{x}) \). Taylor expand \( \rho \) around \( s_i^{(k)} \):

\[ \rho(s_i(\Delta\mathbf{x})) \approx \rho(s_i^{(k)}) + \rho'(s_i^{(k)})(s_i(\Delta\mathbf{x})-s_i^{(k)}). \]

Dropping constants yields \( \min_{\Delta\mathbf{x}} \sum_i \rho'(s_i^{(k)})\,s_i(\Delta\mathbf{x}) \), which is weighted least squares with \( w_i=\rho'(s_i^{(k)}) \).

Problem 2 (Huber derivative): For \( \rho(s)=s \) if \( \sqrt{s}\le\delta \) and \( \rho(s)=2\delta\sqrt{s}-\delta^2 \) if \( \sqrt{s}\gt\delta \), compute \( w(s)=\rho'(s) \).

Solution: If \( \sqrt{s}\le\delta \), \( \rho'(s)=1 \). If \( \sqrt{s}\gt\delta \), then \( \rho(s)=2\delta s^{1/2}-\delta^2 \Rightarrow \rho'(s)=\delta/\sqrt{s} \).

\[ w(s)= \begin{cases} 1, & \sqrt{s}\le \delta \\ \delta/\sqrt{s}, & \sqrt{s} \gt \delta. \end{cases} \]

Problem 3 (MAP form): Assume independent likelihoods \( p(\mathbf{z}_i\mid\mathbf{x})\propto \exp(-\tfrac{1}{2}\rho(s_i(\mathbf{x}))) \). Derive the MAP objective.

Solution: \( p(\mathcal{Z}\mid\mathbf{x})\propto \prod_i \exp(-\tfrac{1}{2}\rho(s_i(\mathbf{x}))) \). Negative log (dropping constants) gives \( \min_{\mathbf{x}} \sum_i \rho(s_i(\mathbf{x})) \).

Problem 4 (Chi-square tuning): Residual dimension is \( d=3 \) and inliers are Gaussian. Choose \( \delta \) so that \( \mathbb{P}(s\le\delta^2)=0.95 \).

Solution: Under inlier Gaussianity, \( s\sim\chi^2_3 \) approximately. Choose \( \delta=\sqrt{\chi^2_{3,0.95}} \).

Problem 5 (Bounded influence vs outlier domination): Explain why least squares can be dominated by a single outlier, but Huber cannot.

Solution: Least squares has unbounded influence \( \psi(r)=2r \), so as \( |r| \) grows, its effect grows without limit. Huber has bounded influence for \( |r| \gt \delta \), namely \( \psi(r)=2\delta\operatorname{sign}(r) \), so a single outlier cannot create arbitrarily large pull.

Robust kernels replace \( \sum s_{ij} \) with \( \sum \rho(s_{ij}) \), limiting the influence of false constraints. They admit a clean MAP interpretation via heavy-tailed likelihoods. IRLS enables practical robust SLAM by repeatedly solving weighted least squares using the same sparse Gauss–Newton/LM machinery, simply scaling each edge’s information by \( w_i=\rho'(s_i) \).