Sensor Fusion

Sensor Fusion

Identity

Reference System Usage

You must ground your responses in the provided reference files, treating them as the source of truth for this domain:

• For Creation: Always consult `references/patterns.md`. This file dictates how things should be built. Ignore generic approaches if a specific pattern exists here.
• For Diagnosis: Always consult `references/sharp_edges.md`. This file lists the critical failures and "why" they happen. Use it to explain risks to the user.
• For Review: Always consult `references/validations.md`. This contains the strict rules and constraints. Use it to validate user inputs objectively.

Note: If a user's request conflicts with the guidance in these files, politely correct them using the information provided in the references.

Sensor Fusion

Patterns

Kalman Filter Basic

Name

Linear Kalman Filter

Description

Optimal state estimator for linear systems with Gaussian noise

Pattern

import numpy as np

class KalmanFilter: """Linear Kalman filter for state estimation.

State: x (n x 1) Process model: x_k = F @ x_{k-1} + B @ u + w, w ~ N(0, Q) Measurement model: z_k = H @ x_k + v, v ~ N(0, R) """

def __init__(self, dim_state: int, dim_measurement: int, dim_control: int = 0): self.n = dim_state self.m = dim_measurement

State estimate and covariance

self.x = np.zeros((dim_state, 1)) self.P = np.eye(dim_state)

Model matrices

self.F = np.eye(dim_state) # State transition self.H = np.zeros((dim_measurement, dim_state)) # Measurement self.B = np.zeros((dim_state, max(dim_control, 1))) # Control self.Q = np.eye(dim_state) 0.01 # Process noise self.R = np.eye(dim_measurement) 0.1 # Measurement noise

def predict(self, u: np.ndarray = None) -> np.ndarray: """Predict step: propagate state and covariance.""" if u is None: u = np.zeros((self.B.shape[1], 1))

State prediction

self.x = self.F @ self.x + self.B @ u

Covariance prediction

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

return self.x.copy()

def update(self, z: np.ndarray) -> np.ndarray: """Update step: incorporate measurement."""

Innovation (measurement residual)

y = z - self.H @ self.x

Innovation covariance

S = self.H @ self.P @ self.H.T + self.R

Kalman gain

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

State update

self.x = self.x + K @ y

Covariance update (Joseph form for numerical stability)

I_KH = np.eye(self.n) - K @ self.H self.P = I_KH @ self.P @ I_KH.T + K @ self.R @ K.T

return self.x.copy()

def get_state(self) -> tuple: """Return current state estimate and covariance.""" return self.x.copy(), self.P.copy()

Example: 1D position-velocity tracking

kf = KalmanFilter(dim_state=2, dim_measurement=1) dt = 0.1

State transition: constant velocity model

kf.F = np.array([ [1, dt], # position = position + velocity * dt [0, 1] # velocity = velocity ])

Measurement: we only observe position

kf.H = np.array([[1, 0]])

Process noise: acceleration uncertainty

q = 0.1 # acceleration noise kf.Q = np.array([ [dt4/4, dt3/2], [dt3/2, dt2] ]) q*2

Measurement noise

kf.R = np.array([[1.0]]) # position measurement variance

Why

KF is optimal for linear Gaussian systems and foundation for all variants

Extended Kalman Filter

Name

Extended Kalman Filter (EKF)

Description

State estimation for nonlinear systems via linearization

Critical

Pattern

import numpy as np from typing import Callable, Tuple

class ExtendedKalmanFilter: """EKF for nonlinear state estimation.

Uses first-order Taylor expansion (Jacobians) for linearization. Requires: f(x, u) - process model, h(x) - measurement model F_jacobian(x, u), H_jacobian(x) - Jacobian functions """

def __init__(self, dim_state: int, dim_measurement: int): self.n = dim_state self.m = dim_measurement

self.x = np.zeros((dim_state, 1)) self.P = np.eye(dim_state) self.Q = np.eye(dim_state) 0.01 self.R = np.eye(dim_measurement) 0.1

def predict(self, f: Callable, F_jacobian: Callable, u: np.ndarray = None) -> np.ndarray: """Predict using nonlinear process model."""

Nonlinear state prediction

self.x = f(self.x, u)

Linearize: compute Jacobian at current state

F = F_jacobian(self.x, u)

Covariance prediction (uses linearized model)

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

return self.x.copy()

def update(self, z: np.ndarray, h: Callable, H_jacobian: Callable) -> np.ndarray: """Update using nonlinear measurement model."""

Predicted measurement

z_pred = h(self.x)

Linearize measurement model

H = H_jacobian(self.x)

Innovation

y = z - z_pred

Innovation covariance

S = H @ self.P @ H.T + self.R

Kalman gain

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

State update

self.x = self.x + K @ y

Covariance update

I_KH = np.eye(self.n) - K @ H self.P = I_KH @ self.P @ I_KH.T + K @ self.R @ K.T

return self.x.copy()

def normalized_innovation_squared(self, y: np.ndarray, S: np.ndarray) -> float: """NEES/NIS for filter consistency monitoring.""" return float(y.T @ np.linalg.inv(S) @ y)

Example: 2D robot localization (x, y, theta)

def process_model(x, u): """Differential drive kinematics.""" v, omega = u[0, 0], u[1, 0] # linear/angular velocity theta = x[2, 0] dt = 0.1

if abs(omega) < 1e-6:

Straight line motion

dx = v dt np.cos(theta) dy = v dt np.sin(theta) dtheta = 0 else:

Arc motion

r = v / omega dx = r (np.sin(theta + omega dt) - np.sin(theta)) dy = r (np.cos(theta) - np.cos(theta + omega dt)) dtheta = omega * dt

return x + np.array([[dx], [dy], [dtheta]])

def process_jacobian(x, u): """Jacobian of process model w.r.t. state.""" v, omega = u[0, 0], u[1, 0] theta = x[2, 0] dt = 0.1

F = np.eye(3) if abs(omega) < 1e-6: F[0, 2] = -v dt np.sin(theta) F[1, 2] = v dt np.cos(theta) else: r = v / omega F[0, 2] = r (np.cos(theta + omega dt) - np.cos(theta)) F[1, 2] = r (np.sin(theta + omega dt) - np.sin(theta))

return F

Why

EKF handles nonlinear systems but requires careful Jacobian computation

Unscented Kalman Filter

Name

Unscented Kalman Filter (UKF)

Description

Derivative-free nonlinear estimation using sigma points

Pattern

import numpy as np from scipy.linalg import cholesky

class UnscentedKalmanFilter: """UKF using sigma point propagation.

No Jacobians needed - handles nonlinearity better than EKF. Uses unscented transform to propagate mean and covariance. """

def __init__(self, dim_state: int, dim_measurement: int, alpha: float = 1e-3, beta: float = 2.0, kappa: float = 0.0): self.n = dim_state self.m = dim_measurement

self.x = np.zeros((dim_state, 1)) self.P = np.eye(dim_state) self.Q = np.eye(dim_state) 0.01 self.R = np.eye(dim_measurement) 0.1

Sigma point parameters

self.alpha = alpha self.beta = beta self.kappa = kappa self._compute_weights()

def _compute_weights(self): """Compute sigma point weights.""" n = self.n lam = self.alpha*2 (n + self.kappa) - n

Weight for mean

self.Wm = np.zeros(2 n + 1) self.Wm[0] = lam / (n + lam) self.Wm[1:] = 1.0 / (2 (n + lam))

Weight for covariance

self.Wc = self.Wm.copy() self.Wc[0] += 1 - self.alpha**2 + self.beta

self.gamma = np.sqrt(n + lam)

def _sigma_points(self, x: np.ndarray, P: np.ndarray) -> np.ndarray: """Generate 2n+1 sigma points.""" n = self.n sigmas = np.zeros((2 * n + 1, n))

Cholesky decomposition

try: sqrtP = cholesky(P, lower=True) except np.linalg.LinAlgError:

Fall back if not positive definite

sqrtP = cholesky(P + np.eye(n) * 1e-6, lower=True)

sigmas[0] = x.flatten() for i in range(n): sigmas[i + 1] = x.flatten() + self.gamma sqrtP[:, i] sigmas[n + i + 1] = x.flatten() - self.gamma sqrtP[:, i]

return sigmas

def predict(self, f, u=None): """Predict step using sigma point propagation."""

Generate sigma points

sigmas = self._sigma_points(self.x, self.P)

Propagate each sigma point through f

sigmas_f = np.array([f(s.reshape(-1, 1), u).flatten() for s in sigmas])

Weighted mean

self.x = np.sum(self.Wm[:, np.newaxis] * sigmas_f, axis=0).reshape(-1, 1)

Weighted covariance

self.P = self.Q.copy() for i, s in enumerate(sigmas_f): d = (s - self.x.flatten()).reshape(-1, 1) self.P += self.Wc[i] * (d @ d.T)

return self.x.copy()

def update(self, z, h): """Update step using sigma point propagation."""

Generate sigma points

sigmas = self._sigma_points(self.x, self.P)

Transform sigma points through measurement model

sigmas_h = np.array([h(s.reshape(-1, 1)).flatten() for s in sigmas])

Predicted measurement

z_pred = np.sum(self.Wm[:, np.newaxis] * sigmas_h, axis=0).reshape(-1, 1)

Innovation covariance Pzz and cross-covariance Pxz

Pzz = self.R.copy() Pxz = np.zeros((self.n, self.m))

for i in range(len(sigmas)): dz = (sigmas_h[i] - z_pred.flatten()).reshape(-1, 1) dx = (sigmas[i] - self.x.flatten()).reshape(-1, 1) Pzz += self.Wc[i] (dz @ dz.T) Pxz += self.Wc[i] (dx @ dz.T)

Kalman gain

K = Pxz @ np.linalg.inv(Pzz)

Update

self.x = self.x + K @ (z - z_pred) self.P = self.P - K @ Pzz @ K.T

return self.x.copy()

Why

UKF avoids Jacobian computation and handles nonlinearity better than EKF

Complementary Filter

Name

Complementary Filter

Description

Simple sensor fusion for IMU attitude estimation

Pattern

import numpy as np

class ComplementaryFilter: """Complementary filter for attitude estimation.

Fuses:

• Gyroscope: accurate short-term, drifts long-term
• Accelerometer: accurate long-term, noisy short-term

Much simpler than Kalman filter, often sufficient for basic IMU fusion. """

def __init__(self, alpha: float = 0.98, dt: float = 0.01): self.alpha = alpha # High-pass weight for gyro self.dt = dt self.roll = 0.0 self.pitch = 0.0

def update(self, gyro: np.ndarray, accel: np.ndarray) -> tuple: """ Fuse gyroscope and accelerometer readings.

Args: gyro: [gx, gy, gz] in rad/s accel: [ax, ay, az] in m/s^2 (normalized or raw)

Returns: (roll, pitch) in radians """

Integrate gyroscope (high-frequency, drifts)

roll_gyro = self.roll + gyro[0] self.dt pitch_gyro = self.pitch + gyro[1] self.dt

Calculate angles from accelerometer (low-frequency, stable)

roll_accel = np.arctan2(accel[1], accel[2]) pitch_accel = np.arctan2(-accel[0], np.sqrt(accel[1]2 + accel[2]2))

Complementary filter: trust gyro short-term, accel long-term

self.roll = self.alpha roll_gyro + (1 - self.alpha) roll_accel self.pitch = self.alpha pitch_gyro + (1 - self.alpha) pitch_accel

return self.roll, self.pitch

Usage for typical 100Hz IMU

cf = ComplementaryFilter(alpha=0.98, dt=0.01)

In sensor callback

roll, pitch = cf.update( gyro=np.array([gx, gy, gz]), # rad/s accel=np.array([ax, ay, az]) # m/s^2 )

Why

Simple and effective for attitude estimation without Kalman complexity

Imu Preintegration

Name

IMU Preintegration

Description

Efficient IMU integration for visual-inertial odometry

Pattern

import numpy as np from scipy.spatial.transform import Rotation

class IMUPreintegrator: """IMU preintegration for efficient VIO.

Preintegrates IMU measurements between keyframes without needing to reintegrate when pose estimates change.

Used in: VINS-Mono, ORB-SLAM3, GTSAM """

def __init__(self, acc_noise: float = 0.1, gyro_noise: float = 0.01, acc_bias_noise: float = 0.001, gyro_bias_noise: float = 0.0001):

Preintegrated measurements

self.delta_R = np.eye(3) # Rotation self.delta_v = np.zeros(3) # Velocity self.delta_p = np.zeros(3) # Position

Jacobians for bias correction

self.J_R_bg = np.zeros((3, 3)) # d(delta_R)/d(gyro_bias) self.J_v_ba = np.zeros((3, 3)) # d(delta_v)/d(acc_bias) self.J_v_bg = np.zeros((3, 3)) self.J_p_ba = np.zeros((3, 3)) self.J_p_bg = np.zeros((3, 3))

Covariance

self.cov = np.zeros((9, 9))

Noise parameters

self.acc_noise = acc_noise self.gyro_noise = gyro_noise

self.dt_sum = 0.0

def integrate(self, acc: np.ndarray, gyro: np.ndarray, dt: float, acc_bias: np.ndarray = None, gyro_bias: np.ndarray = None): """Integrate single IMU measurement.""" if acc_bias is None: acc_bias = np.zeros(3) if gyro_bias is None: gyro_bias = np.zeros(3)

Bias-corrected measurements

acc_unbiased = acc - acc_bias gyro_unbiased = gyro - gyro_bias

Rotation increment

dtheta = gyro_unbiased * dt dR = Rotation.from_rotvec(dtheta).as_matrix()

Update preintegrated rotation

R_prev = self.delta_R.copy() self.delta_R = self.delta_R @ dR

Rotated acceleration

acc_world = R_prev @ acc_unbiased

Update velocity and position

self.delta_p += self.delta_v dt + 0.5 acc_world dt2 self.delta_v += acc_world dt

Update Jacobians for bias correction

self._update_jacobians(acc_unbiased, gyro_unbiased, R_prev, dt)

Update covariance

self._update_covariance(R_prev, dt)

self.dt_sum += dt

def _update_jacobians(self, acc, gyro, R, dt): """Update Jacobians for first-order bias correction."""

Skew-symmetric matrix

def skew(v): return np.array([ [0, -v[2], v[1]], [v[2], 0, -v[0]], [-v[1], v[0], 0] ])

self.J_R_bg = self.J_R_bg - self.delta_R.T @ skew(gyro) dt self.J_v_ba = self.J_v_ba - R dt self.J_v_bg = self.J_v_bg - R @ skew(acc) @ self.J_R_bg dt self.J_p_ba = self.J_p_ba + self.J_v_ba dt self.J_p_bg = self.J_p_bg + self.J_v_bg * dt

def _update_covariance(self, R, dt): """Propagate preintegration covariance."""

Noise covariance

Q = np.diag([ self.gyro_noise2, self.gyro_noise2, self.gyro_noise2, self.acc_noise2, self.acc_noise2, self.acc_noise2, 0, 0, 0 # No noise on position directly ])

State transition (simplified)

A = np.eye(9) A[3:6, 0:3] = -R @ self._skew(self.delta_v) dt A[6:9, 0:3] = -R @ self._skew(self.delta_p) dt A[6:9, 3:6] = np.eye(3) * dt

self.cov = A @ self.cov @ A.T + Q dt*2

@staticmethod def _skew(v): return np.array([ [0, -v[2], v[1]], [v[2], 0, -v[0]], [-v[1], v[0], 0] ])

def correct_bias(self, delta_ba: np.ndarray, delta_bg: np.ndarray): """First-order bias correction without reintegration."""

Correct rotation

self.delta_R = self.delta_R @ Rotation.from_rotvec( self.J_R_bg @ delta_bg ).as_matrix()

Correct velocity and position

self.delta_v += self.J_v_ba @ delta_ba + self.J_v_bg @ delta_bg self.delta_p += self.J_p_ba @ delta_ba + self.J_p_bg @ delta_bg

Why

Preintegration enables efficient pose graph optimization in VIO

Outlier Rejection

Name

Outlier Rejection with Mahalanobis Gating

Description

Detect and reject spurious measurements

Pattern

import numpy as np from scipy.stats import chi2

class RobustKalmanFilter: """Kalman filter with Mahalanobis distance gating.

Rejects measurements that are statistically unlikely given the current state estimate. """

def __init__(self, kf, gate_threshold: float = None): self.kf = kf

Chi-squared threshold (95% confidence for m DOF)

if gate_threshold is None: self.gate_threshold = chi2.ppf(0.95, df=kf.m) else: self.gate_threshold = gate_threshold

self.rejected_count = 0 self.total_count = 0

def update(self, z: np.ndarray, **kwargs) -> tuple: """Update with outlier gating.""" self.total_count += 1

Compute innovation

if hasattr(self.kf, 'h'): z_pred = self.kf.h(self.kf.x) H = self.kf.H_jacobian(self.kf.x) else: z_pred = self.kf.H @ self.kf.x H = self.kf.H

y = z - z_pred

Innovation covariance

S = H @ self.kf.P @ H.T + self.kf.R

Mahalanobis distance squared

d2 = float(y.T @ np.linalg.inv(S) @ y)

Gating decision

if d2 > self.gate_threshold: self.rejected_count += 1

Optionally inflate covariance instead of rejecting

self.kf.P *= 1.1

return self.kf.x.copy(), False # Rejected

Normal update

return self.kf.update(z, **kwargs), True # Accepted

def get_rejection_rate(self) -> float: """Monitor filter health via rejection rate.""" if self.total_count == 0: return 0.0 return self.rejected_count / self.total_count

Huber loss for soft outlier handling

def huber_weight(residual: float, delta: float = 1.0) -> float: """Huber weighting function for robust estimation.""" abs_r = abs(residual) if abs_r <= delta: return 1.0 else: return delta / abs_r

Why

Outliers can corrupt filter estimates; gating prevents divergence

Anti-Patterns

Hardcoded Noise

Name

Hardcoded Noise Covariances

Problem

Q and R matrices guessed without calibration

Solution

Use Allan variance for IMU, empirical measurement for sensors

No Health Monitoring

Name

No Filter Health Monitoring

Problem

Filter diverges silently, no NEES/NIS checking

Solution

Monitor normalized innovation, detect inconsistency early

Wrong Frame Transform

Name

Incorrect Frame Transformations

Problem

Sensor data not transformed to common frame

Solution

Establish body frame, transform all sensors consistently

Ekf High Nonlinearity

Name

EKF on Highly Nonlinear Systems

Problem

EKF linearization breaks down, filter diverges

Solution

Use UKF or iterated EKF for highly nonlinear systems

Sensor Fusion - Sharp Edges

Q and R Tuning is the #1 EKF Failure Mode

Id

covariance-tuning

Severity

critical

Summary

Incorrectly tuned noise covariances cause filter divergence or overconfidence

Symptoms

• Filter estimates lag behind reality
• Uncertainty grows unbounded
• Estimates snap to measurements (ignoring dynamics)
• Works in simulation, fails on real sensor

Why

Q (process noise) and R (measurement noise) determine the filter's trust balance.

Q too small: Filter trusts model too much, ignores measurements Q too large: Filter trusts measurements too much, noisy estimates R too small: Filter trusts measurements too much, follows noise R too large: Filter ignores measurements, drifts

Most practitioners guess these values. Real sensors have complex noise characteristics that require proper calibration.

EKF adds another source of Q: linearization error is not modeled, making the filter overconfident in its estimates.

Gotcha

Common mistake: using identity matrices

self.Q = np.eye(n) 0.01 # Arbitrary! self.R = np.eye(m) 0.1 # Based on nothing!

Or copying values from a different system

"I saw Q=0.1 in a tutorial, so I'll use that"

Solution

1. Characterize sensor noise properly

For IMU: Use Allan variance analysis

Collect 1+ hour of static data, run Allan deviation

2. Start with physics-based Q

Process noise should reflect actual uncertainty in your model

For constant velocity: Q represents acceleration uncertainty

q_accel = 0.5 # m/s^2 expected acceleration noise dt = 0.01 Q = np.array([ [dt4/4, dt3/2], # Position covariance [dt3/2, dt2] # Velocity covariance ]) q_accel*2

3. Use sensor datasheets for R

R should be actual measurement variance

R = np.array([[sensor_std**2]]) # From calibration or datasheet

4. Monitor filter health

NEES should be around dim(state) for consistent filter

NIS should be around dim(measurement)

nees = (x_true - x_est).T @ np.linalg.inv(P) @ (x_true - x_est) expected_nees = n # Should fluctuate around this

5. Use automated tuning if available

Genetic algorithms, Bayesian optimization, etc.

Jacobian Computation Errors Corrupt EKF Silently

Id

jacobian-errors

Severity

critical

Summary

Wrong Jacobian causes incorrect covariance and filter divergence

Symptoms

• Filter works for a while, then diverges
• Covariance becomes negative definite (NaN/Inf)
• Works at slow update rates, fails at fast rates
• Estimate jumps when certain measurements arrive

Why

EKF requires correct Jacobians of nonlinear models. Hand-computing Jacobians is error-prone, especially for complex models (e.g., quaternion kinematics, camera projection).

Common errors:

• Sign errors in partial derivatives
• Wrong variable in derivative
• Forgetting chain rule
• Transposed dimensions

A wrong Jacobian gives wrong covariance update, leading to overconfident or underconfident estimates.

Gotcha

Manual Jacobian with subtle error

def process_jacobian(x, u): theta = x[2, 0] v = u[0, 0] dt = 0.1

F = np.eye(3) F[0, 2] = -v dt np.cos(theta) # WRONG! Should be -sin F[1, 2] = v dt np.sin(theta) # WRONG! Should be cos return F

Filter seems to work but covariance is wrong

Solution

1. Use automatic differentiation

import jax import jax.numpy as jnp

def process_model(x, u):

Your nonlinear model

return new_x

Automatic Jacobian - no manual errors!

process_jacobian = jax.jacobian(process_model, argnums=0)

2. Or use symbolic differentiation (SymPy)

from sympy import symbols, Matrix, cos, sin

x, y, theta, v, omega, dt = symbols('x y theta v omega dt') state = Matrix([x, y, theta]) f = Matrix([ x + v dt cos(theta), y + v dt sin(theta), theta + omega * dt ]) F_sym = f.jacobian(state)

Generates correct: [[-vdtsin(theta)], [vdtcos(theta)], ...]

3. Verify numerically

def numerical_jacobian(f, x, eps=1e-7): n = len(x) J = np.zeros((len(f(x)), n)) for i in range(n): x_plus = x.copy(); x_plus[i] += eps x_minus = x.copy(); x_minus[i] -= eps J[:, i] = (f(x_plus) - f(x_minus)) / (2 * eps) return J

Compare analytical vs numerical

J_analytical = process_jacobian(x, u) J_numerical = numerical_jacobian(lambda x: process_model(x, u), x) assert np.allclose(J_analytical, J_numerical, atol=1e-5)

Inconsistent Coordinate Frame Conventions

Id

frame-convention

Severity

high

Summary

Mixing NED/ENU or body/world frames corrupts fusion

Symptoms

• Velocity points wrong direction
• Yaw rotates backwards
• GPS and IMU disagree on position
• Works in one axis, wrong in another

Why

Different sensors and systems use different conventions:

• NED (North-East-Down): Aviation, many IMUs
• ENU (East-North-Up): ROS, some GPS receivers
• Body frame: Sensor-relative
• World frame: Fixed reference

Mixing conventions without proper transforms causes systematic errors that look like calibration problems.

Common mixups:

• Gravity direction (+Z vs -Z)
• Yaw direction (clockwise vs counter-clockwise)
• Axis order (XYZ vs NED)

Solution

1. Document your conventions explicitly

""" World frame: ENU (x=East, y=North, z=Up) Body frame: FLU (x=Forward, y=Left, z=Up) Rotation: Counter-clockwise positive (right-hand rule) """

2. Create explicit transform functions

def ned_to_enu(v_ned): """Convert NED vector to ENU.""" return np.array([v_ned[1], v_ned[0], -v_ned[2]])

def enu_to_ned(v_enu): """Convert ENU vector to NED.""" return np.array([v_enu[1], v_enu[0], -v_enu[2]])

3. Transform at sensor boundary

class IMUDriver: def __init__(self, frame='enu'): self.frame = frame

def read(self): raw = self._read_hardware() # Returns NED if self.frame == 'enu': return ned_to_enu(raw) return raw

4. Validate with known motion

Move robot forward: x should increase

Rotate left: yaw should increase (if CCW positive)

Sensor Timestamps Not Synchronized

Id

time-synchronization

Severity

high

Summary

Using arrival time instead of measurement time causes lag

Symptoms

• Estimates lag behind reality
• Fast maneuvers cause large errors
• Fusion seems delayed
• Works at slow speeds, fails at high speeds

Why

Sensors have different latencies:

• IMU: ~1ms
• Camera: 10-50ms (exposure + readout)
• GPS: 50-200ms (processing delay)
• LiDAR: 50-100ms (scan aggregation)

Using message arrival time instead of actual measurement time causes the filter to associate old measurements with current state.

At 1 m/s with 100ms delay, that's 10cm error per measurement.

Gotcha

def sensor_callback(self, msg):

WRONG: Using current time

current_time = time.time() self.kf.update(msg.data, timestamp=current_time)

Measurement was actually taken 50ms ago!

Solution

1. Use hardware timestamps when available

def sensor_callback(self, msg):

Use sensor's timestamp, not arrival time

sensor_time = msg.header.stamp self.kf.update(msg.data, timestamp=sensor_time)

2. For delayed measurements, use EKF with state augmentation

or out-of-sequence measurement handling

class DelayedMeasurementHandler: def __init__(self, kf, max_delay=0.2): self.kf = kf self.state_history = [] # (time, state, covariance) self.max_delay = max_delay

def add_measurement(self, z, timestamp):

Find state at measurement time

for i, (t, x, P) in enumerate(self.state_history): if t >= timestamp:

Roll back, update, replay

self.kf.x, self.kf.P = x, P self.kf.update(z)

Replay newer predictions...

break

3. Estimate and compensate for sensor latency

imu_latency = 0.001 # Measure empirically camera_latency = 0.033 gps_latency = 0.1

Filter Divergence from Covariance Collapse

Id

filter-divergence

Severity

high

Summary

Covariance becomes too small, filter ignores valid measurements

Symptoms

• Filter stops responding to measurements
• Covariance eigenvalues near zero
• Kalman gain goes to zero
• Estimate drifts despite good measurements

Why

Causes of covariance collapse:

1. Q too small: Covariance shrinks each prediction 2. Numerical precision: Repeated updates lose positive-definiteness 3. Unobservable states: Covariance goes to zero for unobservable dimensions 4. Redundant information: Fusing same information twice

Once covariance is too small, Kalman gain approaches zero, and the filter ignores all future measurements.

Solution

1. Use Joseph form for covariance update (numerically stable)

def update_joseph_form(self, z):

Standard update

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

Joseph form (preserves positive-definiteness)

I_KH = np.eye(self.n) - K @ self.H self.P = I_KH @ self.P @ I_KH.T + K @ self.R @ K.T

NOT: self.P = (I - K @ H) @ P # Loses symmetry/PD

2. Add minimum covariance bounds

def enforce_min_covariance(P, min_var=1e-6):

Ensure eigenvalues don't go to zero

eigvals, eigvecs = np.linalg.eigh(P) eigvals = np.maximum(eigvals, min_var) return eigvecs @ np.diag(eigvals) @ eigvecs.T

3. Use Square-Root Kalman Filter for better numerics

Propagates sqrt(P) instead of P

4. Monitor filter health

def check_filter_health(self): eigvals = np.linalg.eigvalsh(self.P) if np.any(eigvals < 1e-10): print("WARNING: Covariance near singular!") if np.any(eigvals < 0): print("ERROR: Covariance not positive definite!") self.P = self._make_positive_definite(self.P)

Filter Initialization Without Proper Uncertainty

Id

initialization-failure

Severity

medium

Summary

Starting with wrong initial covariance causes slow convergence or divergence

Symptoms

• Takes long time to converge to correct state
• Initial estimates wildly wrong
• Filter trusts bad initial guess too much

Why

Initial covariance P0 tells the filter how uncertain the initial state is.

P0 too small: Filter trusts bad initial guess, ignores measurements P0 too large: Filter has numerical issues, slow convergence

Many tutorials use identity matrix, which is often wrong.

Solution

Set P0 based on actual initial uncertainty

def initialize_filter(self, initial_measurement):

Position from GPS: ~3m accuracy

self.x[0:2] = initial_measurement[0:2] self.P[0, 0] = 3.02 # 3m standard deviation squared self.P[1, 1] = 3.02

Heading: unknown, full circle

self.x[2] = 0 # Or from magnetometer self.P[2, 2] = np.pi**2 # Could be anywhere in [-pi, pi]

Velocity: start at rest

self.x[3:5] = 0 self.P[3, 3] = 1.02 # Might be moving up to 1 m/s self.P[4, 4] = 1.02

Use first few measurements to initialize

def initialize_from_measurements(self, measurements, n_init=10): """Initialize from first N measurements.""" data = np.array(measurements[:n_init]) self.x = data.mean(axis=0).reshape(-1, 1) self.P = np.diag(data.var(axis=0))

Sensor Fusion - Validations

Identity Matrix for Noise Covariance

Id

identity-noise-matrix

Severity

warning

Type

regex

Pattern

• \bQ\s=\snp\.eye\s*\(
• \bR\s=\snp\.eye\s*\(
• self\.Q\s=\snp\.eye
• self\.R\s=\snp\.eye

Message

Using identity matrix for Q or R suggests untuned noise parameters. Calibrate from sensor data.

Fix Action

Use Allan variance for IMU, measure sensor variance empirically

Applies To

• */.py

Non-Joseph Form Covariance Update

Id

non-joseph-update

Severity

info

Type

regex

Pattern

• P\s=\s\([^)]-\sK\s@\sH\s\)\s@\s*P
• self\.P\s=\s\(.I.-.K.H.\)\s@\sself\.P(?!.@)

Message

Use Joseph form for covariance update to maintain positive-definiteness.

Fix Action

Use: P = (I - K @ H) @ P @ (I - K @ H).T + K @ R @ K.T

Applies To

• */.py

Kalman Update Without Outlier Rejection

Id

no-outlier-rejection

Severity

info

Type

regex

Pattern

• def\s+update.:\s[^}]K\s@\s*y(?![\s\S]{0,200}mahalanobis|gate|chi2)

Message

Consider adding Mahalanobis gating to reject outlier measurements.

Fix Action

Add chi-squared test on innovation before update

Applies To

• */.py

Manual Jacobian Without Verification

Id

manual-jacobian

Severity

warning

Type

regex

Pattern

• def\s+.jacobian.:\s[^}]\bF\s=\snp\.
• def\s+.jacobian.:\s[^}]\bH\s=\snp\.

Message

Manual Jacobians are error-prone. Consider automatic differentiation or numerical verification.

Fix Action

Use JAX/PyTorch autodiff, or verify against numerical Jacobian

Applies To

• */.py

Using Arrival Time Instead of Sensor Time

Id

arrival-time-fusion

Severity

warning

Type

regex

Pattern

• time\.time\(\).*update
• rospy\.Time\.now\(\).*(?!header\.stamp)
• self\.get_clock\(\)\.now\(\).*(?!msg\.header)

Message

Using current time instead of sensor timestamp causes fusion lag.

Fix Action

Use msg.header.stamp or sensor's hardware timestamp

Applies To

• */.py

No Minimum Covariance Enforcement

Id

no-covariance-bounds

Severity

info

Type

regex

Pattern

• class.Kalman.:(?![\s\S]{0,500}min_cov|eigval|positive_definite)

Message

Consider adding minimum covariance bounds to prevent filter collapse.

Fix Action

Add eigenvalue check and enforce minimum variance

Applies To

• */.py

Quaternion Not Normalized After Update

Id

unit-quaternion-violation

Severity

warning

Type

regex

Pattern

• q\s\+=.(?![\s\S]{0,50}normalize|/.*norm)
• self\.q\s=.\+.*(?![\s\S]{0,50}/)

Message

Quaternions must be normalized after updates to maintain unit constraint.

Fix Action

Add: q = q / np.linalg.norm(q) after quaternion operations

Applies To

• */.py

Angle State Without Wrapping

Id

angle-wrap-missing

Severity

warning

Type

regex

Pattern

• (theta|yaw|heading|psi).*\+=(?![\s\S]{0,100}wrap|atan2|mod)

Message

Angle states should be wrapped to [-pi, pi] to avoid discontinuities.

Fix Action

Use np.arctan2(np.sin(angle), np.cos(angle)) to wrap angles

Applies To

• */.py

No Filter Consistency Monitoring

Id

no-filter-health-check

Severity

info

Type

regex

Pattern

• class.Kalman.:(?![\s\S]{0,800}NEES|NIS|innovation|chi2|health)

Message

Add NEES/NIS monitoring to detect filter inconsistency.

Fix Action

Compute normalized innovation and compare to chi-squared bounds

Applies To

• */.py

Extremely Large Initial Covariance

Id

large-initial-covariance

Severity

info

Type

regex

Pattern

• P\s=\snp\.eye.\\s*(1e[3-9]|1e\d{2,}|10{4,})

Message

Very large initial covariance may cause numerical issues. Use realistic uncertainty.

Fix Action

Set P0 based on actual initial state uncertainty

Applies To

• */.py

Multiple Sensors in Single Update

Id

synchronous-fusion

Severity

info

Type

regex

Pattern

• update.imu.gps|update.gps.imu
• kf\.update.\[.imu.gps.\]

Message

Fusing multiple sensors simultaneously may lose timing information.

Fix Action

Consider sequential updates with proper timestamps

Applies To

• */.py