Google Cartographer decouples state estimation into a local real-time front-end and a global asynchronous back-end. The front-end accumulates range scans into local grid submaps using high-frequency Ceres-based non-linear scan matching, while the back-end optimizes a pose graph of submaps using branch-and-bound loop closure verification.

1. Local Front-End (Scan-to-Submap Matching)

Incoming LiDAR scans $P = \{p_i\}_{i=1}^K$ in frame coordinates are mapped into the local submap coordinate frame via rigid transform $T_{\xi} = (R_\theta, t)$ parameterized by pose $\xi = (p_x, p_y, \theta)$. The scan matcher solves a non-linear least squares problem minimizing grid probability residuals using Ceres Solver:

where $M_{\text{smooth}}(x)$ is a bicubicly-interpolated grid cell probability value. To prevent local minima divergence during aggressive motion, initial guess $\xi_{\text{odom}}$ is provided by high-rate IMU pre-integration or wheel odometry.

// Ceres Cost Function for 2D Scan Matching against Grid Submap
struct ScanMatcherCostFunction {
    ScanMatcherCostFunction(const SensorScan& scan, const ProbabilityGrid& grid)
        : scan_(scan), grid_(grid) {}

    template <typename T>
    bool operator()(const T* const pose, T* residual) const {
        T x = pose[0], y = pose[1], theta = pose[2];
        for (size_t i = 0; i < scan_.points.size(); ++i) {
            T px = x + cos(theta) * scan_.points[i].x - sin(theta) * scan_.points[i].y;
            T py = y + sin(theta) * scan_.points[i].x + cos(theta) * scan_.points[i].y;
            T probability = grid_.GetBicubicProbability(px, py);
            residual[i] = T(1.0) - probability;
        }
        return true;
    }
    const SensorScan& scan_;
    const ProbabilityGrid& grid_;
};

2. Submap Generation & Lifecycles

A submap starts empty as a probability grid (typically $5\text{cm}$ resolution). Each accepted scan updates grid probability via odds ratio updates $O(x) = M(x)/(1-M(x))$: $O_{\text{new}}(x) = O_{\text{old}}(x) \cdot O_{\text{hit/miss}}$. To ensure computational tractability, a submap transitions to "finished" once $N_{\text{scans}} \approx 100-200$ frames are inserted. At any time, two submaps are concurrently active: one accumulating scans and one being finalized.

3. Global Back-End & Branch-and-Bound Loop Closure

When a submap is finalized, it becomes a node in the global pose graph. Cartographer searches for loop closure constraints by matching recent scans against all historic finalized submaps within a search window $W$. To make global grid search real-time without GPU acceleration, it employs Real-Time Correlative Scan Matching accelerated by a Branch-and-Bound (B&B) tree search on a multi-resolution grid pyramid:

• Grid Pyramid: Precomputes coarse grids where cell value $M_{\text{coarse}}(x) = \max_{y \in \text{window}} M(y)$.
• Bounding: The score of any scan in a coarse cell bounds the maximum achievable score of all child cells in the fine grid. If $Score_{\text{coarse}} < Score_{\text{best\_known}}$, the entire branch is pruned.

4. Pose Graph Optimization with Huber Robust Loss

Upon accepting candidate loop closure relative transforms $T_{i,j}$, Ceres minimizes the global graph cost function:

where $\rho(r)$ is the Huber Robust Loss function ($\rho(r) = r^2$ for $|r|\le \delta$, $2\delta|r|-\delta^2$ for $|r|>\delta$). The Huber loss down-weights erroneous loop closure constraints (false positives from perceptual aliasing), preventing map destruction.

A robust GPS-denied Return-To-Home (RTH) architecture relies on Visual-Inertial Odometry (VIO) keyframe graph navigation rather than absolute global coordinates. The drone records an active $SE(3)$ pose graph during outbound flight and retraces it during emergency return.

1. System State Architecture & Data Structure

During normal outbound flight (GPS healthy or VIO nominal), the autonomy stack logs a lightweight topological pose graph at $2\text{Hz}$:

struct KeyframeNode {
    uint32_t id;
    double timestamp;
    Eigen::Matrix4d T_world_robot; // SE(3) Pose
    std::vector<cv::KeyPoint> keypoints;
    cv::Mat descriptors;           // DBoW2 / NetVLAD feature vector
    pcl::PointCloud<pcl::PointXYZ>::Ptr local_point_cloud;
};
std::vector<KeyframeNode> trajectory_graph;

2. Failover State Machine & Traversal

Upon triggering GPS loss / link disconnection:

1. State Transition: Switch EKF3 input from GPS position measurements to tight-coupled VIO (stereo RealSense D435i + onboard IMU) producing relative pose updates $\Delta T \in SE(3)$ at $30\text{Hz}$.
2. Heading Inversion: Command a $180^\circ$ yaw rotation in place while maintaining altitude via laser/baro altimeter.
3. Path Inversion & Smoothing: Reverse the keyframe trajectory list. Apply B-spline interpolation across keyframe positions to generate smooth velocity targets $v_{\text{cmd}}(t)$ for the local trajectory planner (DWA / TEB).
4. Visual Drift Correction (Relocalization): As the drone flies backward along the flight path, run feature matching (ORB/SuperPoint + RANSAC PnP) against stored keyframe descriptors. Detect re-visited keyframes to apply instant pose corrections, eliminating dead-reckoning integration drift.

State estimation algorithms trade off computational complexity, non-linear representation accuracy, and smoothing capabilities across past states.

1. Extended Kalman Filter (EKF)

EKF linearizes non-linear dynamics $f(x, u)$ and measurement models $h(x)$ using first-order Taylor series expansions around the current estimate state $\hat{x}_k$ via Jacobian matrices $F_k = \frac{\partial f}{\partial x}\big|_{\hat{x}_{k-1}}$ and $H_k = \frac{\partial h}{\partial x}\big|_{\hat{x}_k^-}$:

Limitation: Unimodal Gaussian assumption. Once a state update is finalized at step $k$, past estimates cannot be revised upon receiving out-of-order measurements or loop closures.

2. Unscented Kalman Filter (UKF)

UKF avoids computing analytical Jacobians. It deterministically samples $2L+1$ Sigma Points $\chi_i$ around state mean $\mu$ with covariance $P$ using the Unscented Transform ($\gamma = \sqrt{L + \lambda}$):

Sigma points are passed directly through the exact non-linear function $Y_i = f(\chi_i)$, capturing 2nd-order Taylor series terms. Compute cost scales as $\mathcal{O}(L^3)$ due to matrix square root (Cholesky decomposition) per update step.

3. Factor Graph Optimization (Smoothing / iSAM2)

Maintains a graph where nodes represent robot states $X = \{x_0, \dots, x_N\}$ and edges represent non-linear factor costs $f_k(X_k)$. Solves the full joint Maximum A Posteriori (MAP) inference via non-linear least squares:

Using iSAM2 (incremental Smoothing and Mapping), factor graphs convert non-linear factors into a Bayes Tree, updating only affected variables during incremental steps, allowing past state revision upon loop closure.

4. Why ArduPilot Selected EKF3

ArduPilot runs its primary flight controller loop on STM32 microcontrollers (Cortex-M7 at $216\text{MHz}$). EKF3 maintains a 24-state vector (Position, Velocity, Quaternion Attitude, Gyro/Accel Biases, Earth Magnetic Field, Body Magnetic Field, Wind Velocity). The EKF prediction/update cycle completes in $<150\mu\text{s}$, fitting tightly within the $1\text{kHz}$ RTOS task loop. Factor graphs are computationally impossible on bare-metal MCU hardware due to dynamic memory requirements and unbounded optimization execution times.

Visual-Inertial Odometry (VIO) degrades rapidly under optical occlusion, low texture, or IR oversaturation. Robust estimation architectures prevent filter divergence via Mahalanobis distance measurement gating and covariance inflation management.

1. Outlier Gating via Mahalanobis Distance

Before fusing an incoming visual landmark observation $z_k$, the estimator computes the residual covariance $S_k = H_k P_k^- H_k^T + R_k$ and innovation $\nu_k = z_k - h(\hat{x}_k^-)$. The squared Mahalanobis distance $d_M^2$ is evaluated against a Chi-Squared $\chi^2$ distribution gate threshold:

If $d_M^2 > \chi^2$ (e.g., due to lens smudge or partial occlusion producing corrupted feature positions), the measurement is rejected instantly, preventing corrupted updates from pulling the state estimate off track.

2. Covariance Growth During Blind Dead-Reckoning

When complete visual dropout occurs (e.g., passing through smoke or optical sensor failure), the filter stops applying measurement update equations ($K_k = 0$). State propagation relies exclusively on IMU accelerometer and gyroscope integration:

In the absence of correction updates $R_k$, the position uncertainty covariance $P_{pp}$ grows exponentially with time $t^3$ (due to double-integration of uncorrected accelerometer bias $b_a$).

3. Visual Relocalization & State Recovery Sequence

// Pseudocode: VIO Recovery State Machine
if (tracked_features_count < MIN_FEATURES_THRESHOLD) {
    estimator_status = VIO_TRACKING_LOST;
    // Freeze visual updates, rely on IMU + Baro dead reckoning
    fuse_visual_updates = false;
} else if (estimator_status == VIO_TRACKING_LOST) {
    // Attempt 3D-2D PnP RANSAC alignment against keyframe map
    Eigen::Matrix4d T_recovery;
    if (solve_pnp_ransac(current_frame, keyframe_database, T_recovery)) {
        // Reset Error State EKF covariance and re-align state vector
        esekf.reset_attitude_and_position(T_recovery);
        fuse_visual_updates = true;
        estimator_status = VIO_NOMINAL;
    }
}

Multi-rate sensor fusion requires an Error-State Kalman Filter (ESKF) coupled with a temporal circular buffer to handle asynchronous measurement latency without blocking high-rate prediction.

1. Error-State Kalman Filter Split

The state is split into true state $x$, nominal state $\tilde{x}$, and error state $\delta x$ ($x = \tilde{x} \oplus \delta x$):

• Nominal State $\tilde{x}$ ($1\text{kHz}$): Propagated asynchronously by high-rate IMU readings via raw kinematic integration (non-linear manifold integration). No covariance calculations in this fast path.
• Error State $\delta x$ (Asynchronous): Linearized error dynamics $\delta \dot{x} = F_{\text{err}} \delta x + w$. Covariance $P$ is updated only when lower-rate sensors arrive.

2. Delayed Measurement Handling via Ring Buffer

To fuse a camera measurement arriving at $t_{\text{now}}$ with timestamp $t_{\text{cam}} = t_{\text{now}} - 80\text{ms}$:

1. Maintain a circular buffer of historical nominal states $\tilde{x}(\tau)$ and covariance matrices $P(\tau)$ for the past $200\text{ms}$.
2. Retrieve stored state $\tilde{x}(t_{\text{cam}})$ and $P(t_{\text{cam}})$ corresponding to frame capture time.
3. Compute innovation $\nu = z_{\text{cam}} - h(\tilde{x}(t_{\text{cam}}))$ and Kalman Gain $K$.
4. Update error state $\delta x(t_{\text{cam}}) = K \nu$ and inject back into nominal state: $\tilde{x}_{\text{corrected}}(t_{\text{cam}}) = \tilde{x}(t_{\text{cam}}) \oplus \delta x(t_{\text{cam}})$.
5. Re-propagate nominal state from $t_{\text{cam}}$ to $t_{\text{now}}$ using stored IMU readings in the buffer.