IMU Preintegration and High-Rate Sensor Fusion
Running an EKF at IMU rate (200 Hz) with a 10 Hz LIDAR is architecturally wrong — IMU preintegration fixes this without approximation.
🎯 By the end you'll understand
Why running a single EKF at IMU rate is architecturally incorrect. How preintegration collapses many IMU readings between two LIDAR keyframes into a single, exact measurement. The manifold geometry that makes preintegration work on SO(3). How gyroscope and accelerometer bias corrupt the preintegrated measurement — and why the corruption grows quadratically with time. The 9-DOF VIO state vector and how the preintegrated term enters as a single observation in the LIDAR-rate filter. The ROS 2 implementation pattern for FollowBot.
KEY INSIGHTThe rate mismatch is not a performance problem — it is a correctness problem
You cannot simply run your EKF at 200 Hz using IMU measurements and then accept a LIDAR update whenever one arrives. That approach quietly violates the statistical assumptions the filter is built on.
FollowBot's IMU fires at 200 Hz. Its LIDAR fires at 10 Hz. The naive approach — run the EKF prediction step 200 times a second and accept LIDAR updates at 10 Hz — seems reasonable on the surface. It is not. The EKF's prediction step advances the state with a process noise covariance Q that is designed for one time step. Doing 20 IMU predictions between every LIDAR update inflates the uncertainty in a way that the filter's covariance math does not correctly track. More insidiously, every one of those 20 predictions re-linearizes around the current state, accumulating linearization error. The filter stays "converged" in appearance while its covariance matrix becomes physically meaningless.
IMU preintegration is the principled fix. Instead of running the filter at IMU rate, you integrate the raw IMU measurements separately, accumulating a single compound measurement — the preintegrated delta-position, delta-velocity, and delta-rotation — that can be injected as one update into a LIDAR-rate filter. The math is exact. The filter runs at 10 Hz. The IMU information is fully preserved.
In a properly implemented preintegration scheme, the uncertainty of the compound measurement is computed analytically from the IMU noise model. This means the LIDAR-rate EKF receives not just a position/rotation delta but a calibrated covariance matrix telling it exactly how much to trust that delta. A naive 200 Hz EKF cannot provide this — its covariance has been corrupted by 20 sequential linearizations. The practical consequence: the naive approach either over-trusts the IMU (leading to LIDAR updates being down-weighted unnecessarily) or under-trusts it (leading to filter divergence when LIDAR returns fail). Preintegration gives you the right answer.
CORE CONCEPTThree ways naive high-rate fusion breaks the EKF
Let us be precise about why the naive approach fails. There are three distinct problems, and each has a different root cause.
Linearization accumulation
The EKF prediction step computes a Jacobian F of the motion model at the current state estimate. On a curved trajectory — FollowBot turning while accelerating — each of the 20 IMU-rate Jacobians is computed at a slightly different operating point. The first-order approximation error at each step is small, but 20 sequential small errors compound. The error in the predicted covariance grows with the number of prediction steps between corrections, not with the elapsed time. A filter running at 200 Hz between 10 Hz corrections accumulates 20× the linearization error of a filter that only runs 1 prediction step between corrections. Preintegration re-derives the exact compound rotation using the rotation group SO(3) directly, avoiding this accumulation entirely.
Process noise scaling
The process noise matrix Q in an EKF is calibrated for a specific prediction timestep Δt. At 200 Hz, Δt = 5 ms; at 10 Hz, Δt = 100 ms. A Q calibrated for 5 ms prediction steps cannot be simply reused for the full 100 ms interval between LIDAR updates — the noise accumulates over the integration period and must be computed as the integral of the noise power spectral density over time. The naive approach of running Q twenty times at 5 ms intervals does produce the correct total noise if the system is linear. But the IMU integration is not linear: the rotation couples position and velocity in a nonlinear way. The correct compound noise covariance for the preintegrated measurement requires analytical propagation through the rotation manifold.
Re-linearization on bias change
When LIDAR updates cause a sudden correction to the IMU bias estimate (gyroscope bias, accelerometer bias), a naive 200 Hz EKF must rewind and recompute all 20 predictions since the last LIDAR measurement — which is computationally expensive and often skipped. In a preintegration scheme, the preintegrated measurement carries a Jacobian with respect to the bias estimate. When the bias changes after a LIDAR update, the preintegrated delta is corrected analytically using this Jacobian, at constant cost regardless of how many IMU samples were integrated. This is what makes preintegration practical in real-time systems.
HOW IT WORKSPreintegration on the manifold: the math
The IMU measures angular velocity ω̃ (gyroscope) and specific force f̃ (accelerometer) in the body frame. In continuous time, ignoring bias and noise for the moment:
In a world frame W, given body-frame measurements:
Ṙ = R · [ω]×
v̇ = R · f − g
ṗ = v
where R ∈ SO(3) is the rotation from body to world, v is world-frame velocity, p is world-frame position, g is the gravity vector, and [ω]× is the skew-symmetric matrix of ω. Integrating these three equations from keyframe time ti to tj gives the relative motion between keyframes.
The insight behind preintegration is that the right-hand sides mix world-frame quantities (R, v, p) with body-frame measurements (ω̃, f̃). If you integrate naively, you need to know R at every instant — which means re-integrating from scratch every time the initial pose estimate is updated by a LIDAR correction.
The preintegration trick: factor out the dependency on the initial rotation Ri. Define the preintegrated deltas:
ΔRij = RiT · Rj (relative rotation in body frame)
Δvij = RiT · (vj − vi − g · Δtij) (relative velocity)
Δpij = RiT · (pj − pi − vi · Δtij − ½ g · Δtij2) (relative position)
These three quantities depend only on body-frame IMU measurements between ti and tj. They can be computed once from the raw IMU data and reused regardless of how the global pose estimate changes.
In discrete time, the preintegration recurrence accumulates one IMU sample at a time:
import numpy as np from scipy.spatial.transform import Rotation class IMUPreintegrator: # Accumulates IMU samples between two LIDAR keyframes. # All quantities are in the body frame of the START keyframe. def __init__(self, bg, ba): # bg: gyroscope bias (rad/s), ba: accelerometer bias (m/s²) self.bg = np.array(bg) # shape (3,) self.ba = np.array(ba) # shape (3,) self.reset() def reset(self): self.dR = np.eye(3) # ΔR (3×3 rotation matrix) self.dv = np.zeros(3) # Δv (m/s, body frame of start) self.dp = np.zeros(3) # Δp (m, body frame of start) self.dt = 0.0 # accumulated time (s) def integrate(self, omega_raw, accel_raw, dt): # Subtract bias before integration omega = omega_raw - self.bg accel = accel_raw - self.ba # Rotate accumulated delta-v and delta-p by current dR, add accel self.dp = self.dp + self.dv * dt + 0.5 * (self.dR @ accel) * dt**2 self.dv = self.dv + (self.dR @ accel) * dt # Rotate dR by this timestep's rotation (right-multiply on SO(3)) dR_step = Rotation.from_rotvec(omega * dt).as_matrix() self.dR = self.dR @ dR_step self.dt += dt
The key line is self.dR = self.dR @ dR_step: rotation is composed on the manifold using matrix multiplication, not added as Euler angles. This is why preintegration is described as integration "on the manifold" — SO(3) is not a vector space and you cannot integrate angular velocity by adding rotation components.
Rotations in 3D do not commute: rotating 90° about X then 90° about Y gives a different result than 90° about Y then 90° about X. The vector space shortcut — treat orientation as a 3-vector and add increments — silently violates this, accumulating gimbal-lock artifacts and normalization drift. Using matrix multiplication on SO(3) (or equivalently, quaternion multiplication) is not an academic preference; it is the difference between a filter that stays calibrated and one that slowly drifts into singularities on aggressive maneuvers.
The preintegrated covariance ΣΔ is propagated analytically alongside the delta states, using a 9×9 noise propagation matrix derived from the linearized IMU kinematics. When the preintegrated delta is injected into the LIDAR-rate EKF as an observation, this covariance serves as the measurement noise matrix R in the Kalman gain computation — giving the filter an exact, well-calibrated uncertainty.
SEE ITIMU preintegration visualizer
This widget simulates a FollowBot moving between two LIDAR keyframes. The IMU fires at 200 Hz; the keyframe interval is 100 ms (20 samples). Adjust the gyroscope bias slider and observe how the preintegrated trajectory drifts from ground truth. The drift is quadratic in time — bias causes angular error that compounds into position error. The worst choice you can make: leaving gyroscope bias unestimated and hoping the EKF compensates.
The quadratic drift is not a coincidence. Gyroscope bias θ̇err causes a linearly growing heading error: θerr(t) = θ̇err · t. That heading error rotates the integrated accelerometer output, causing a position error that grows as ½ · a · t² · sin(θerr) ≈ ½ · a · θ̇err · t² for small angles. Position drift is quadratic in time. At 0.05 rad/s bias, over the 100 ms keyframe interval the position error is already several centimeters — comparable to the LIDAR's scan-matching accuracy — and it grows rapidly if the LIDAR updates stop (during a dropout or a self-similar corridor with poor scan-matching).
Accelerometer bias is subtracted before computing the specific force. But the accelerometer also sees the gravity vector rotated into the body frame — if the orientation estimate is wrong by θerr, the gravity projection error is g · sin(θerr) ≈ 9.81 · θerr m/s². At θerr = 1°, this is a spurious 0.17 m/s² acceleration injected into every IMU sample. Over 100 ms, this contributes 0.85 mm of position error per keyframe — small but accumulating. This cross-coupling between gyroscope accuracy and accelerometer apparent noise is why IMU calibration always estimates both biases simultaneously, not independently.
CORE CONCEPTBias estimation: the hidden variable the filter must track
The preintegration equations above assumed known bias vectors bg (gyroscope) and ba (accelerometer). In practice, these biases are unknown and time-varying. They drift slowly with temperature (MEMS gyroscopes drift at roughly 1–10 °/hr, depending on grade), and there is a turn-on transient that can take 10–30 seconds to settle. You cannot calibrate them once and forget — you must estimate them continuously.
The standard approach is to add bg and ba as state variables in the EKF, with a random-walk process model:
# Bias process model: each bias component follows Brownian motion # Q_bias = sigma_w² * I * dt (sigma_w: bias walk rate, e.g. 1e-5 rad/s/√s) Q_bg = (sigma_bg_walk**2) * np.eye(3) * dt # gyro bias walk covariance Q_ba = (sigma_ba_walk**2) * np.eye(3) * dt # accel bias walk covariance # In EKF prediction, bias state is held constant (mean), covariance grows: # bg_k+1 = bg_k + noise_walk ~ N(0, Q_bg) # The Kalman filter will pull the bias estimate toward consistency # each time a LIDAR update constrains the absolute pose.
The crucial feature of the preintegration bias Jacobians mentioned earlier now pays off. The preintegrated delta (ΔR, Δv, Δp) was computed with the current bias estimate. When the EKF's LIDAR update changes the bias estimate slightly, you do not need to re-integrate 20 IMU samples. Instead:
# After EKF update gives new bias estimate (bg_new, ba_new): delta_bg = bg_new - bg_old # small change in gyro bias delta_ba = ba_new - ba_old # small change in accel bias # First-order correction using precomputed Jacobians: dR_corrected = dR_old @ Rotation.from_rotvec(J_R_bg @ delta_bg).as_matrix() dv_corrected = dv_old + J_v_bg @ delta_bg + J_v_ba @ delta_ba dp_corrected = dp_old + J_p_bg @ delta_bg + J_p_ba @ delta_ba # This is O(1) — no re-integration. The Jacobians J_* were computed # incrementally during the original preintegration pass.
This is the engineering payoff that makes preintegration worth the mathematical overhead. In a naive 200 Hz EKF, a bias correction would require replaying the last 20 samples — a 20× compute spike at every LIDAR update. With preintegration, it is a fixed-cost matrix multiply.
The bias state is observable only when the robot moves in ways that create independent information about the bias. A robot driving a perfectly straight line at constant velocity cannot distinguish gyroscope bias from a slight misalignment of the wheel encoders — both look like a constant drift in heading. Bias becomes observable when the robot turns (gyro bias manifests as heading error that the LIDAR disagrees with) and when acceleration varies (accel bias manifests as position offset that LIDAR scan-matching rejects). In practice, this means FollowBot's bias estimates are unreliable during long stretches of straight driving in featureless corridors — exactly the scenario where LIDAR scan-matching also degrades. Two hard problems compound.
DEFINITIONThe 9-DOF VIO state vector
Visual-Inertial Odometry (VIO) — and by analogy, LIDAR-Inertial Odometry (LIO) — extends the basic pose state vector to include IMU biases. The resulting state vector has 9 degrees of freedom per keyframe:
x = [ pW (3 DOF — world-frame position)
vW (3 DOF — world-frame velocity)
RBW (3 DOF — rotation body→world, parameterized as SO(3))
bg (3 DOF — gyroscope bias, body frame)
ba (3 DOF — accelerometer bias, body frame) ]
Total: 15 scalar unknowns (p, v contribute 3 each; R contributes 3 via its Lie algebra; bg, ba contribute 3 each). In EKF implementations, the rotation is often carried as a quaternion (4 scalars with a unit-norm constraint) but the error-state formulation uses a 3-vector perturbation δθ, keeping the innovation space minimal.
The preintegrated measurement (ΔRij, Δvij, Δpij) enters as an observation at the keyframe boundary. The measurement function h(x) maps the state vector to predicted preintegrated deltas:
def preint_residual(state_i, state_j, preint): # Extract states at keyframe i and j R_i, v_i, p_i = state_i.R, state_i.v, state_i.p R_j, v_j, p_j = state_j.R, state_j.v, state_j.p g = np.array([0, 0, -9.81]) dt = preint.dt # Predicted preintegrated quantities from state variables dR_hat = R_i.T @ R_j dv_hat = R_i.T @ (v_j - v_i - g * dt) dp_hat = R_i.T @ (p_j - p_i - v_i * dt - 0.5 * g * dt**2) # Residuals: measured - predicted (on the manifold for rotation) r_R = Log(preint.dR.T @ dR_hat) # SO(3) log map → R³ r_v = preint.dv - dv_hat r_p = preint.dp - dp_hat return np.concatenate([r_R, r_v, r_p]) # 9-vector residual
The H matrix (Jacobian of h with respect to the state) for this 9-vector residual is derived analytically — it is a 9×15 matrix (or 9×(6+6+3) in the error-state formulation). Computing it correctly for the SO(3) rotation part requires the left- and right-Jacobians of the rotation group, which is where most textbooks become dense. The physical intuition: a small rotation error δθ in Ri rotates the entire preintegrated trajectory, producing a predictable (and linear, to first order) error in the predicted Δp and Δv.
| Quantity | Symbol | DOF | Updated by | Observability condition |
|---|---|---|---|---|
| World-frame position | pW | 3 | LIDAR scan-match | Feature-rich environment |
| World-frame velocity | vW | 3 | IMU preintegration | Non-constant acceleration |
| Body-to-world rotation | RBW | 3 | LIDAR + gravity alignment | Always (with gravity) |
| Gyroscope bias | bg | 3 | LIDAR pose consistency | Robot must rotate |
| Accelerometer bias | ba | 3 | LIDAR pose consistency | Robot must accelerate |
FOLLOWBOTImplementing preintegrated IMU fusion on FollowBot
FollowBot's full EKF milestone (state vector [x, y, θ, v, ω] fusing wheel odometry + IMU + LIDAR) was designed in earlier lessons. Preintegration extends the architecture: instead of feeding the IMU directly into the EKF prediction step, you add an IMU preintegration node that runs at full 200 Hz and publishes a preintegrated measurement message whenever a LIDAR scan-match completes.
The ROS 2 implementation uses a custom message type for the preintegrated measurement and a synchronized callback that triggers whenever a scan-match result arrives:
import rclpy from rclpy.node import Node from sensor_msgs.msg import Imu from nav_msgs.msg import Odometry from followbot_msgs.msg import PreintegratedIMU # custom msg import numpy as np class PreintegratorNode(Node): def __init__(self): super().__init__('imu_preintegrator') self.preint = IMUPreintegrator(bg=[0,0,0], ba=[0,0,0]) # IMU sub at full rate (200 Hz) self.create_subscription(Imu, '/imu/data', self.imu_cb, 10) # LIDAR scan trigger — when a scan arrives, publish and reset self.create_subscription(Odometry, '/scan_match_pose', self.scan_trigger_cb, 10) self.pub = self.create_publisher(PreintegratedIMU, '/imu/preintegrated', 10) self.last_t = None def imu_cb(self, msg): t = msg.header.stamp.sec + msg.header.stamp.nanosec * 1e-9 if self.last_t is None: self.last_t = t; return dt = t - self.last_t; self.last_t = t omega = np.array([msg.angular_velocity.x, msg.angular_velocity.y, msg.angular_velocity.z]) accel = np.array([msg.linear_acceleration.x, msg.linear_acceleration.y, msg.linear_acceleration.z]) self.preint.integrate(omega, accel, dt) def scan_trigger_cb(self, msg): # Publish accumulated delta, then reset for next interval out = PreintegratedIMU() out.delta_t = self.preint.dt out.delta_p = self.preint.dp.tolist() out.delta_v = self.preint.dv.tolist() # flatten 3×3 rotation to 9 floats out.delta_r = self.preint.dR.flatten().tolist() self.pub.publish(out) self.preint.reset()
The Stage 4 milestone specifies full EKF operation with position error below 0.1 m RMS and NEES consistency validation. Without preintegration, the naive 200 Hz EKF approach inflates the covariance matrix with 20× accumulated linearization error per LIDAR interval, causing NEES to be systematically below n (the state dimension; n=5 for FollowBot's filter) — the filter is underconfident (P too large, covariance over-estimates the true error). With preintegration, the EKF runs at 10 Hz with a well-calibrated measurement covariance from the preintegrated IMU, and NEES stays near n under standard maneuvers. If you find NEES consistently low in your recorded data, check whether you are naively fusing IMU at high rate — this is the most common cause of inflated covariance in IMU-fused EKFs.
WEAKNESSWhere IMU preintegration bites
This is the failure mode this lesson was designed to demonstrate. If you implement preintegration but treat gyroscope bias as zero (or as a fixed calibration constant), the preintegrated delta-rotation accumulates error proportional to the true bias magnitude times the integration time. That rotation error tilts the integrated accelerometer output, causing position drift that grows as t² — not linearly. In a 100 ms keyframe window the error may be sub-centimeter. But if LIDAR scan-matching fails for 1–2 seconds (entering a featureless corridor, glass walls, rapid rotation), the position error grows 100× to 10,000× the per-frame value. The robot's state estimate diverges while the filter's covariance may still appear small, because the filter's uncertainty model was also constructed with the wrong bias. The fix is always the same: include bg and ba in the EKF state vector and let LIDAR updates constrain them.
The preintegrated measurement covariance ΣΔ is a 9×9 matrix. Many implementations simplify it to a diagonal matrix — assigning independent variances to each component of ΔR, Δv, Δp. This is incorrect. On a curved trajectory, errors in ΔR directly contaminate Δp (a rotation error rotates the integrated position). The off-diagonal terms ∂Δp/∂δθ and ∂Δv/∂δθ are numerically significant for any trajectory with turning. Using a diagonal ΣΔ tells the EKF that rotation uncertainty and position uncertainty are independent, causing it to under-weight LIDAR corrections in exactly the situations (sharp turns) where the preintegrated IMU is most uncertain.
Preintegration integrates IMU samples from keyframe time ti to keyframe time tj. If the IMU and LIDAR clocks are not hardware-synchronized, the effective integration window is misaligned by the clock offset — typically 1–5 ms on USB-connected sensors without hardware triggering. At 200 Hz IMU rate, a 5 ms offset corresponds to one full IMU sample being misattributed to the wrong keyframe interval. The resulting delta measurement is contaminated with inter-frame motion that the filter cannot account for. In production, FollowBot should use hardware trigger lines or the chrony PTP (Precision Time Protocol) daemon to align sensor clocks to sub-millisecond accuracy. Verify clock alignment before trusting any preintegration-based filter.
KEY INSIGHTHow this connects
- The Extended Kalman Filter: Nonlinear Systems and Jacobian Linearization (4.5) — preintegration is an extension of the EKF prediction step. Understanding how the EKF Jacobian F is computed at each step (Lesson 4.5) makes the preintegration derivation concrete: we are replacing 20 sequential linearizations with one exact compound rotation on SO(3).
- Sensors: Physics, Noise Models, and Failure Modes (1.6) — the IMU noise model (white noise density σg, σa and bias walk rate σg,walk, σa,walk) is the input to the preintegrated covariance propagation. The Allan variance characterization method taught in Lesson 1.6 produces the exact noise parameters needed here.
- Multi-Sensor Fusion on FollowBot: Architecture and Covariance Design (4.9) — the next lesson assembles the complete multi-sensor fusion stack: how the preintegrated IMU measurement, LIDAR scan-match, and wheel encoder odometry are combined in a single EKF with correct covariance design for each sensor input.
IMU preintegration is not an optional optimization. It is the principled architecture for fusing any high-rate dead-reckoning sensor (IMU, wheel encoders) with a low-rate absolute sensor (LIDAR, camera, GPS). The same mathematics appear in visual-inertial odometry (VIO) systems like VINS-Mono and OKVIS, and in LiDAR-inertial systems like LIO-SAM. Understanding it at this level means you can adapt any of those systems to FollowBot's sensor configuration without treating them as black boxes.
GLOSSARYKey terms introduced
- IMU preintegration
- The technique of collapsing many high-rate IMU measurements between two low-rate sensor keyframes into a single compound measurement (ΔR, Δv, Δp) with analytically computed covariance. Allows the state estimator to run at the low-rate sensor's frequency while preserving all IMU information.
- Preintegrated delta
- The triple (ΔRij, Δvij, Δpij) representing the relative rotation, velocity change, and position change between keyframes i and j, expressed in the body frame of keyframe i. Computed entirely from body-frame IMU measurements and independent of the global pose estimate.
- SO(3)
- The special orthogonal group in 3 dimensions — the set of 3×3 orthogonal matrices with determinant +1. The mathematical space of 3D rotations. Rotation increments must be composed by matrix multiplication (not vector addition), which is the defining feature of integration "on the manifold."
- Gyroscope bias (bg)
- A slowly time-varying offset in the gyroscope output, typically caused by MEMS fabrication imperfections and temperature-dependent drift. Unestimated gyroscope bias causes heading error that grows linearly in time and position error that grows quadratically — the dominant failure mode in IMU-based dead-reckoning.
- Bias Jacobian
- The partial derivative of a preintegrated delta (ΔR, Δv, or Δp) with respect to the gyroscope or accelerometer bias. Computed incrementally during preintegration; used to first-order-correct the preintegrated measurement after the EKF updates the bias estimate, avoiding costly re-integration.
- Keyframe
- A timestamped state snapshot at which a slow-rate absolute sensor (LIDAR, camera) produces a measurement. IMU measurements between consecutive keyframes are preintegrated into a single compound observation. The EKF runs at the keyframe rate, not the IMU rate.
- NEES (Normalized Estimation Error Squared)
- A filter consistency metric: NEES = (x̂ − xtrue)T P−1 (x̂ − xtrue). For a consistent filter, NEES follows a chi-squared distribution with expectation equal to the state dimension n. Systematic NEES above n means overconfident (P too small); systematic NEES below n means underconfident (P too large). Naive high-rate IMU fusion inflates P (accumulated linearization error), producing NEES < n — an underconfident EKF. Preintegration produces a well-calibrated covariance and restores NEES ≈ n.
- LIO-SAM / VIO
- LiDAR-Inertial Odometry via Smoothing and Mapping; Visual-Inertial Odometry. Production SLAM systems that use IMU preintegration as a core component to tightly couple high-rate inertial measurements with low-rate range or camera observations in a factor graph or EKF framework.
ACTIVE RECALLCheck your understanding
Answer in your own words first, then reveal.
1Why can't you simply run an EKF prediction step at 200 Hz (IMU rate) and accept LIDAR updates at 10 Hz? Name two distinct problems this causes.›
Problem 1 — Linearization accumulation: the EKF prediction step re-linearizes the motion model at each step. On a curved trajectory, doing 20 sequential linearizations between LIDAR updates accumulates first-order approximation error proportional to the number of steps, not the elapsed time. The covariance matrix becomes physically incorrect. Problem 2 — Bias re-integration cost: when a LIDAR update changes the IMU bias estimate, a naive 200 Hz EKF must replay all 20 recent IMU predictions with the corrected bias — a 20× compute spike that is usually skipped in practice, silently leaving a stale bias in the predictions. Preintegration solves both: it computes an exact compound rotation via SO(3) matrix multiplication, and it carries bias Jacobians that allow O(1) first-order correction of the preintegrated delta after any bias update.
2Explain in physical terms why gyroscope bias causes position error that grows quadratically with time, not linearly.›
A constant gyroscope bias θ̇err causes the estimated heading to drift at a constant rate: θerr(t) = θ̇err · t — linear in time. That heading error rotates the accelerometer measurement out of alignment with the true direction of motion. The accelerometer's contribution to estimated position is R(θ) · a, where R(θ) is the rotation by the estimated heading. When θ has a linear error, this rotation is wrong, and the projected acceleration is misaligned by an amount that grows linearly with time. Integrating a misaligned acceleration twice (first to velocity, then to position) produces position error ∝ t². Physically: the robot thinks it is driving straight while actually curving, and the gap between its estimated straight-line path and its true curved path widens quadratically. At 100 ms, the error is small. After 1 second without LIDAR correction, it is 100× larger.
3What does "preintegration on the manifold" mean, and why does it matter relative to integrating angular velocity as a 3-vector?›
"On the manifold" means integrating rotation increments using the group multiplication rule of SO(3) — composing rotation matrices by matrix multiplication: Rnew = Rold · Exp(ω · dt). The alternative — treating orientation as a 3-vector (Euler angles or rotation vector) and adding increments — is only valid for infinitesimal angles and breaks down for finite rotations because SO(3) is not a vector space: rotations do not commute, and repeated addition of finite angle vectors does not represent a rotation in a geometrically consistent way. Concretely, naive vector integration drifts off the rotation manifold (produces non-orthogonal matrices), accumulates gimbal-lock-like singularities near ±180° rotations, and introduces systematic errors proportional to the angular rate squared times the timestep. Using SO(3) matrix multiplication is exact for constant angular velocity and has error only O(Δω · Δt²) for slowly varying angular rate — dramatically better than vector integration for the aggressive maneuvers FollowBot makes when tracking a running person.
4FollowBot's NEES test shows values consistently around 0.3 during IMU-heavy motion. What is the most likely cause, and how does preintegration address it?›
NEES consistently around 0.3 — well below the state dimension n=5 — means the filter is underconfident (P too large: the covariance over-estimates the true error). The most likely cause during IMU-heavy motion is accumulated linearization error from naive high-rate IMU prediction: running 20 EKF prediction steps per LIDAR interval, each with accumulated compound-rotation error, inflates the predicted covariance beyond what the true error warrants. The fix is IMU preintegration: instead of 20 prediction steps, one preintegrated measurement is injected per keyframe with a calibrated covariance ΣΔ that correctly accounts for the full compound rotation uncertainty (including off-diagonal coupling between rotation and position error). After switching to preintegration, the EKF's prediction covariance growth is well-calibrated, and NEES should return to approximately n under normal maneuvers.