Jump to
MODULE 05 // SENSOR FUSION

Sensor Fusion

No single sensor tells the whole truth. An IMU is fast but drifts. A camera is rich in information but blind in darkness. A LiDAR is precise but power-hungry. Radar sees through smoke but has poor resolution. Sensor fusion is the discipline of combining all of them β€” weighing each by how trustworthy it is at any given moment β€” to produce a navigation solution better than any sensor alone could provide.

Bayesian Fusion IMU + Camera + LiDAR Temporal Alignment Chi-Square Fault Detection

πŸ“š  REFERENCES: MDPI Autonomous Control Ch.3  Β·  Bestaoui Sebbane Β§8  Β·  Sadraey Β§6  Β·  Halesworth Β§4

5.1 Why Fuse Multiple Sensors?

Plain English First

Imagine navigating at night in fog with no GPS. You have a compass (tells you direction), a speedometer (tells you speed), a radar altimeter (tells you height above ground), and a camera (shows you the ground below). Each one gives you partial, imperfect information. The compass drifts. The speedometer lags. The altimeter bounces with terrain. The camera is useless in pure darkness. But combined intelligently, they constrain each other β€” the camera corrects the compass, the altimeter bounds the speedometer's accumulated error, and the radar fills in when the camera fails. Sensor fusion is the science of combining partial, imperfect, asynchronous information sources to produce something better than any single source.

Sensor fusion in UAV navigation is not optional β€” it is architecturally mandatory. MDPI Autonomous Control identifies three fundamental reasons any single sensor is insufficient for military UAV navigation: MDPI Autonomous Control Ch.3

Coverage Gaps

Every sensor has conditions under which it fails completely. GPS fails under jamming. Camera fails in darkness or smoke. LiDAR fails in rain. No single sensor works in all military environments. Fusion creates redundancy β€” when one sensor fails, others compensate.

Complementarity

Different sensors observe different aspects of the state. IMU measures acceleration (fast, noisy). Camera measures appearance (slow, rich). LiDAR measures geometry (precise, expensive). Together they constrain the full state vector more tightly than any single modality.

Rate Mismatch

Control loops need state at 1000 Hz. Cameras deliver 30 Hz. LiDAR delivers 10 Hz. GPS delivers 1–10 Hz. The IMU fills the gaps between slow sensor updates, providing the high-rate "bridge" that all other sensors correct during their infrequent measurements.

Analogy β€” An Experienced Ship's Navigator

Before GPS, a ship's navigator used dead reckoning (compass + log speed), celestial navigation (star angles), depth soundings (terrain matching), and radio beacons β€” each unreliable alone, but together constraining the ship's position to within a nautical mile even after days at sea. They mentally weighted each source by how long ago it was taken, what the weather was, and how much they trusted the instrument. That mental process is exactly what a sensor fusion algorithm formalises mathematically.

5.2 Sensor Modalities β€” Characteristics and Error Models

Plain English First

Before you can fuse sensors, you must deeply understand each one β€” not just what it measures, but how it fails. A sensor's noise model is the mathematical description of its errors. Getting this wrong is the number one cause of filter divergence in practice. An overconfident noise model (thinking the sensor is more accurate than it is) causes the filter to trust bad measurements and go wrong. An underconfident model (thinking it's less accurate than it is) causes the filter to ignore good data and drift unnecessarily.

⟳ MEMS IMU
Rate: 100–4000 Hz  Β·  Latency: <1 ms
Noise model: White noise + bias random walk + temperature drift
Strengths: Extremely fast; dead reckoning bridge; attitude rate
Failure modes: Vibration aliasing; g-sensitivity; thermal shock
Drift: Position error ∝ t² (quadratic with no correction)
Always-on High-rate
β—‰ Monocular Camera
Rate: 20–120 Hz  Β·  Latency: 10–50 ms
Noise model: Reprojection ~0.5 px; rolling shutter distortion
Strengths: Rich texture; loop closure; feature tracking
Failure modes: Motion blur; low texture; illumination extremes
Scale: Unobservable without known baseline or IMU integration
Scale-ambiguous Passive
⊹ 3D LiDAR
Rate: 10–20 Hz  Β·  Latency: 50–100 ms
Noise model: ~2 cm range noise; ~0.1Β° angular noise
Strengths: Direct 3D structure; no scale ambiguity; dense mapping
Failure modes: Rain/dust backscatter; specular surfaces; range limits
Power: 5–15 W (heavy for Group 1–2 UAVs)
Scale-resolved Power-hungry
βŒ– Radar Altimeter
Rate: 10–100 Hz  Β·  Latency: <5 ms
Noise model: Β±0.3–1.5% of altitude; multipath in urban areas
Strengths: Works in total darkness, fog, rain, smoke
Failure modes: Specular ground (smooth water); steep angles; urban multipath
Military use: Terrain-following, SITAN, low-level ingress
All-weather Active RF
β–² Barometer
Rate: 25–100 Hz  Β·  Latency: <5 ms
Noise model: ~1–3 m altitude noise; slow thermal drift
Strengths: Very low cost; lightweight; passive; altitude reference
Failure modes: Prop wash pressure shock; icing; ventilation effects
Drift: Weather front passage can shift by 10–30 m over hours
Lightweight Relative alt.
β—ˆ Magnetometer
Rate: 10–100 Hz  Β·  Latency: <5 ms
Noise model: Heading error ~1–5Β°; hard/soft iron distortion
Strengths: Absolute heading reference (no drift); passive
Failure modes: Ferrous structures; motor EMI; geographic variation
Military note: Motor currents on multirotors can corrupt reading by 10–30Β°
EMI-sensitive Absolute hdg.

Allan Variance β€” Quantifying IMU Quality

The IMU is the backbone of all fusion architectures β€” it runs continuously and bridges every other sensor update. Its quality is quantified by Allan variance analysis per IEEE Standard 952. The log-log plot of Allan deviation $\sigma_A(\tau)$ vs. averaging time $\tau$ reveals three physically distinct noise regions: Bestaoui Sebbane Β§8.1

Allan Variance Decomposition β€” IEEE Std 952 $$\sigma_A^2(\tau) = \underbrace{\frac{N^2}{\tau}}_{\text{Angle/Vel RW}} + \underbrace{B^2}_{\text{Bias instability}} + \underbrace{K^2\tau}_{\text{Rate RW}}$$

To extract parameters: run the IMU stationary for $>$6 hours. Compute Allan deviation. Read off: $N$ at slope $-1/2$ (short $\tau$), $B$ at the minimum (medium $\tau$), $K$ at slope $+1/2$ (long $\tau$). These three numbers directly populate the EKF process noise matrix $Q$ β€” making Allan variance the essential pre-flight calibration procedure for any serious navigation system.

Worked Example 5.1 β€” Reading Allan Variance for EKF Tuning

An ADIS16488 tactical-grade IMU is characterised. Allan variance analysis yields: $N = 0.3Β°/\sqrt{\text{hr}}$, $B = 6.5Β°/\text{hr}$, $K = 0.15Β°/\text{hr}/\sqrt{\text{hr}}$ (gyro). Convert to EKF process noise for a 100 Hz loop ($\Delta t = 0.01$ s).

Allan ParameterRaw ValueSI UnitsEKF $Q$ Entry
$N$ (angle RW)0.3°/√hr$1.455\times10^{-4}$ rad/√s$Q_{\psi\psi} = N^2 \cdot \Delta t = 2.12\times10^{-9}$ rad²
$B$ (bias instab.)6.5Β°/hr$3.15\times10^{-5}$ rad/sFloor on gyro bias estimate noise
$K$ (rate RW)0.15°/hr/√hr$7.27\times10^{-7}$ rad/s/√s$Q_{b_g b_g} = K^2 \cdot \Delta t = 5.28\times10^{-15}$ rad²/s²

Key insight: Every entry in the EKF process noise matrix $Q$ has a physical origin traceable to the sensor's Allan variance parameters. Setting $Q$ by guessing is the most common EKF tuning mistake in practice. Setting $Q$ from Allan variance measurements is the systematic, defensible approach used in certified military navigation systems. Bestaoui Sebbane Β§8.2

5.3 Fusion Architectures β€” Loose, Tight, and Deep Coupling

Plain English First

There are three ways to combine the outputs of multiple sensors, ranging from simple to sophisticated. The simplest is loose coupling: run each sensor's own filter to get a position estimate, then combine those estimates. Tight coupling skips the intermediate step β€” raw sensor measurements go directly into a single shared filter. Deep coupling goes further still: the filter actively feeds information back into the sensor processing itself, helping it work better. Each step up requires more engineering but delivers meaningfully better performance, especially when sensors start failing.

Loose Coupling

Sensor outputs
GPS β†’ position
Camera β†’ pose
Baro β†’ altitude
Individual filters
GPS EKF
+
VIO filter
+
Baro filter
Fusion layer
Navigation EKF (fuses position estimates)
β†’
State estimate

Advantage: Modular β€” each sensor subsystem can be developed and tested independently. Easy to add or remove sensors. Disadvantage: The intermediate position estimates are correlated (they share IMU data), but this correlation is ignored when combining them β€” leading to overconfident (too-small) covariance estimates and potential filter divergence. MDPI Autonomous Control Ch.3

Tight Coupling

Raw measurements
IMU raw accel/gyro
Camera features (pixels)
LiDAR point cloud
Baro pressure
Single filter
Single Navigation EKF/UKF (all raw measurements in one state)
β†’
State estimate + covariance

Advantage: Correct correlation handling β€” all measurements share the same state, so their dependencies are properly modelled. Can maintain output even when individual subsystems have too few observations to produce their own fix (e.g., GPS with only 2 satellites visible instead of 4). Disadvantage: Requires deriving measurement models for raw sensor outputs β€” much more complex implementation. Bestaoui Sebbane Β§8.3

Deep Coupling

Deep coupling closes the loop β€” the navigation filter's state estimate feeds back into the sensor processing itself: Bestaoui Sebbane Β§8.4

Bidirectional
Navigation EKF
⇄
IMU pre-integration
Feedback paths EKF attitude estimate β†’ IMU bias correction before integration
EKF velocity estimate β†’ aids camera feature search window
EKF position estimate β†’ constrains LiDAR scan matching initial guess

Deep coupling is used in navigation-grade military systems (HG9900 class) and advanced research systems. The IMU pre-integration residual approach (used in VINS-Mono, ORB-SLAM3) is a form of deep coupling where IMU data is pre-processed into pose-relative measurements before entering the smoother. Halesworth Β§4.2

Theorem 5.1 β€” Why Tight Coupling Outperforms Loose in GNSS-Denied Scenarios

In loose coupling, each subsystem filter requires a minimum number of valid measurements to produce an output. For GPS, this is typically 4+ satellites. For VIO, typically 5+ tracked features. When these minimums are not met, the subsystem produces no output β€” and the navigation filter receives no update.

In tight coupling, raw measurements enter the shared EKF directly. With only 2 GPS pseudoranges available (below the 4-satellite minimum for position fix), those 2 measurements still provide 2 constraints on the state β€” reducing position uncertainty along two dimensions even without a full fix. Similarly, 3 tracked camera features (below the 5-feature minimum for monocular VO) still contribute partial constraints. The information is never wasted.

Formally: tight coupling processes $N$ raw measurements providing $N$ independent constraints, regardless of whether they reach any individual subsystem threshold. Loose coupling provides either zero or all-or-nothing updates. In degraded environments where sensors intermittently drop below their fix threshold, tight coupling provides continuous (if reduced) observability. Bestaoui Sebbane Β§8.3

5.4 Covariance Modelling β€” Getting the Uncertainty Right

Plain English First

The Kalman filter's performance depends entirely on how accurately you model the uncertainty of each sensor and each state. The covariance matrix $P$ tells the filter how uncertain it is about its own estimate. The measurement noise matrix $R$ tells it how uncertain the sensors are. If either is wrong β€” even by an order of magnitude β€” the filter produces poorly calibrated estimates: overconfident ones diverge, underconfident ones drift. Getting these matrices right is an art backed by physics, calibration data, and flight test.

The Process Noise Matrix Q

$Q$ encodes uncertainty injected into the state by the motion model during each predict step. For an IMU-driven navigation EKF, $Q$ is populated from Allan variance parameters: Bestaoui Sebbane Β§8.5

Process Noise Q from IMU Allan Variance $$Q_{vv} = \sigma_a^2 \cdot \Delta t \cdot I_3, \quad Q_{pp} = \frac{1}{3}\sigma_a^2 \cdot \Delta t^3 \cdot I_3$$ $$Q_{b_g b_g} = \sigma_{bg}^2 \cdot \Delta t \cdot I_3, \quad Q_{b_a b_a} = \sigma_{ba}^2 \cdot \Delta t \cdot I_3$$

$\sigma_a$: accelerometer noise density [m/sΒ²/√Hz]; $\sigma_{bg}, \sigma_{ba}$: gyro/accel bias random walk noise [rad/s/√Hz, m/sΒ²/√Hz]. Position noise grows as $\Delta t^3$ (triple integration of acceleration noise β€” this is the mathematical origin of the quadratic drift discussed in Module 04). All other components scale linearly with $\Delta t$ β€” they represent noise added per timestep.

The Measurement Noise Matrix R

$R$ encodes sensor measurement uncertainty. It must capture not just white noise but also any systematic biases or correlations in the measurement: Sadraey Β§6.2

Measurement Noise Models for Common UAV Sensors $$R_{GPS} = \begin{bmatrix}\sigma_{E}^2 & 0 & 0 \\ 0 & \sigma_{N}^2 & 0 \\ 0 & 0 & \sigma_{D}^2\end{bmatrix}, \quad \sigma_E = \sigma_N \approx 3\text{–}5\text{ m}, \quad \sigma_D \approx 5\text{–}10\text{ m}$$ $$R_{VIO} = \sigma_{px}^2 \cdot I_2, \quad \sigma_{px} \approx 0.5\text{ pixel} \cdot \frac{R_s}{f}$$ $$R_{baro} = \sigma_{alt}^2, \quad \sigma_{alt} \approx 1\text{–}3\text{ m (SL)}$$

GPS is worse in the vertical (Down) direction than horizontal β€” the satellite geometry is always above the horizon, giving poor vertical dilution of precision (VDOP). VIO reprojection noise scales with slant range $R_s$ and inversely with focal length $f$ β€” a telephoto camera at altitude gives better VIO than a wide-angle at low altitude. Barometer noise increases with altitude due to lower air density and turbulence.

Adaptive Covariance Estimation

In real operations, sensor noise characteristics change: a camera degrades in smoke, GPS degrades under electronic attack. Static $R$ matrices become wrong. Adaptive methods update $R$ in real time: MDPI Autonomous Control Ch.3

Innovation-Based Adaptive Estimation (IAE) $$\hat{R}_k = (1-\alpha)\hat{R}_{k-1} + \alpha\left(\mathbf{y}_k \mathbf{y}_k^T - H_k P_{k|k-1} H_k^T\right)$$

$\mathbf{y}_k = \mathbf{z}_k - h(\hat{\mathbf{x}}_{k|k-1})$: innovation vector; $\alpha \in (0,1)$: forgetting factor (typically 0.95–0.99). The logic: if the innovation is consistently larger than expected (sensor degrading), the estimated $\hat{R}_k$ grows, reducing trust in the sensor automatically. If the sensor improves (GPS signal recovers), $\hat{R}_k$ shrinks and trust is restored. This is used in the PX4 EKF2 module for GPS health monitoring. Halesworth Β§4.3

5.5 Temporal Alignment β€” Every Millisecond Matters

Plain English First

Sensor fusion assumes all measurements refer to the same point in time. In reality, sensors produce data at different rates, with different delays, and with different timestamp accuracy. The IMU samples every 1 ms. The camera delivers a frame every 33 ms but the image was actually captured 10 ms ago (processing delay). The LiDAR spins mechanically, so different points in a single scan were acquired at different moments. If you ignore these time differences and treat all measurements as simultaneous, you inject a systematic position error proportional to vehicle speed times the timing error. At 15 m/s, a 50 ms misalignment causes 75 cm of systematic position error β€” larger than VIO's noise level. Temporal alignment is not optional.

Time Alignment Error β€” Position Consequence $$\delta p_{timing} = \mathbf{v} \cdot \delta t$$

$\mathbf{v}$: vehicle velocity [m/s]; $\delta t$: timestamp error [s]. At 55 m/s cruise and 20 ms software timestamp jitter (typical Linux without RT patch): $\delta p = 55 \times 0.02 = 1.1$ m systematic error per measurement. Since this error is systematic (not random), it does not average out β€” it biases the state estimate consistently. Hardware trigger + GPS-disciplined oscillator brings $\delta t < 1\,\mu$s, giving $\delta p < 55\,\mu$m β€” negligible. Bestaoui Sebbane Β§8.6

Strategies for Time Alignment

StrategyMechanismAchievable $\delta t$CostUsed In
Software timestampOS records time on data receipt1–20 ms (OS jitter)FreeConsumer drones, ROS2 default
Hardware triggerGPIO pulse triggers camera + IMU simultaneously1–100 ΞΌsLowResearch UAVs, PX4 AUX pins
PPS syncGPS pulse-per-second disciplines local oscillator<1 ΞΌsMediumMilitary navigation-grade systems
Time-offset estimationEKF estimates camera-IMU time offset as a state~1 ms residualSoftware onlyKalibr calibration, VINS-Mono
Rolling shutter correctionModel per-row exposure time with IMU rotation~0.1 ms residualSoftwareMobile/embedded cameras

Extrinsic Calibration β€” Sensor-to-Body Frame

In addition to time alignment, every sensor has a rigid spatial offset from the vehicle body frame β€” the extrinsic transform ${}^B T_S \in SE(3)$: Bestaoui Sebbane Β§8.7

Lever Arm Effect β€” IMU to Camera Offset $$\mathbf{p}_{cam} = \mathbf{p}_{IMU} + R_{B}^{W} \cdot \mathbf{r}_{IMU \to cam}^B + R_{B}^{W} \cdot (\boldsymbol{\omega} \times \mathbf{r}_{IMU \to cam}^B) \cdot \delta t$$

$\mathbf{r}_{IMU \to cam}^B$: position of camera relative to IMU in body frame (lever arm). The third term is the lever arm velocity β€” when the UAV is rotating at angular rate $\boldsymbol{\omega}$, the camera is physically moving relative to the IMU, even if the vehicle's centre of mass is stationary. For a 15 cm lever arm and 1 rad/s yaw rate: lever arm velocity = $0.15$ m/s. Over a 0.1 s camera exposure: 15 mm additional position error if uncorrected. This is why extrinsic calibration must account for both translation and rotation.

5.6 Sensor Fault Detection and Isolation

Plain English First

GPS spoofing gives the receiver a confident but wrong position. A camera feature mismatch produces a large but confident-looking correction. A magnetometer disturbed by motor currents gives a plausible but systematically biased heading. In each case, the sensor is technically working β€” it's just measuring the wrong thing. Without fault detection, the fusion filter will accept these bad measurements as truth and corrupt the entire navigation solution. Fault detection algorithms act as a gatekeeper β€” rejecting measurements that are statistically inconsistent with the current state estimate before they can corrupt it.

The Chi-Square Innovation Test

The most widely used fault detection method in navigation is the chi-square test on the innovation (also called RAIM β€” Receiver Autonomous Integrity Monitoring in GPS context): MDPI Autonomous Control Ch.3

Chi-Square Innovation Test β€” RAIM $$\epsilon_k = \mathbf{y}_k^T S_k^{-1} \mathbf{y}_k \sim \chi^2(m) \quad \text{under nominal conditions}$$ $$\text{Accept if: } \epsilon_k \leq \chi^2_{m,\, 1-\alpha} \qquad \text{Reject (fault) if: } \epsilon_k > \chi^2_{m,\, 1-\alpha}$$

$\mathbf{y}_k$: innovation vector; $S_k$: innovation covariance; $m$: measurement dimension; $\alpha$: false alarm probability (typically $10^{-3}$ to $10^{-5}$ for military systems). Under correct filter assumptions, the normalised innovation squared $\epsilon_k$ follows a chi-squared distribution with $m$ degrees of freedom. If a measurement is faulty, $\epsilon_k$ will be anomalously large β€” exceeding the threshold for the chosen $\alpha$. The threshold $\chi^2_{m,\,1-\alpha}$ is looked up from chi-squared tables.

Worked Example 5.2 β€” Chi-Square GPS Spoofing Detection

The EKF predicts position (100.0, 200.0) m with covariance $P_{pos} = \text{diag}(4, 4)$ mΒ². GPS noise $R = \text{diag}(9, 9)$ mΒ². A GPS reading arrives: (105.0, 203.0) m. Is this a legitimate measurement or possible spoofing? Use $\alpha = 0.001$ (0.1% false alarm).

Step 1 β€” Innovation: $\mathbf{y} = [105.0 - 100.0,\; 203.0 - 200.0] = [5.0,\; 3.0]$ m

Step 2 β€” Innovation covariance: $S = H P H^T + R = \text{diag}(4,4) + \text{diag}(9,9) = \text{diag}(13, 13)$ mΒ²

Step 3 β€” Normalised innovation squared: $$\epsilon = \mathbf{y}^T S^{-1} \mathbf{y} = \frac{5.0^2}{13} + \frac{3.0^2}{13} = \frac{25 + 9}{13} = \frac{34}{13} = \mathbf{2.615}$$

Step 4 β€” Chi-square threshold: For $m=2$ measurements and $\alpha = 0.001$: $\chi^2_{2,\,0.999} = 13.82$

Decision: $\epsilon = 2.615 < 13.82$ β†’ ACCEPT. The measurement is consistent. The 5 m position jump is large but within the combined prediction and measurement uncertainty ($\sqrt{13} \approx 3.6$ m standard deviation).

Spoofing scenario: If GPS instead reads (130.0, 240.0) m (30 m and 40 m jump): $\epsilon = (30^2 + 40^2)/13 = 5000/13 = 384.6 \gg 13.82$ β†’ REJECT. The measurement is declared a fault. The filter continues on its IMU prediction without updating.

Sensor Fault Isolation β€” Which Sensor Failed?

The chi-square test detects that something is wrong. Isolation identifies which sensor is faulty. The standard approach is the FDIE (Fault Detection, Isolation, and Exclusion) method: run the chi-square test with each sensor excluded in turn. The excluded sensor that most reduces $\epsilon_k$ is the most likely culprit. Once isolated, the sensor is flagged as degraded and its contribution is reduced (increased $R$) or removed entirely, with a re-check after a timeout period. Bestaoui Sebbane Β§8.8

5.7 Information-Theoretic View of Sensor Fusion

Plain English First

How much does a new sensor measurement actually help? Sometimes a measurement barely reduces your uncertainty β€” maybe you already knew the altitude to within 1 metre and the new barometer reading adds nothing. Other times, a single GPS fix after 5 minutes of dead reckoning collapses your uncertainty from 200 metres down to 5 metres. Information theory gives us a precise way to quantify "how much did I learn from this measurement?" β€” and that quantity directly determines whether it was worth the weight, power, and complexity of carrying that sensor.

Fisher Information and the CramΓ©r-Rao Bound

The Fisher Information Matrix (FIM) quantifies how much each measurement constrains the state estimate. Its inverse is the minimum achievable covariance β€” the CramΓ©r-Rao Bound (CRB): MDPI Autonomous Control Ch.3

Fisher Information Matrix and CramΓ©r-Rao Bound $$\mathcal{I}(\mathbf{x}) = \mathbb{E}\left[\left(\frac{\partial \log p(\mathbf{z}|\mathbf{x})}{\partial \mathbf{x}}\right)\left(\frac{\partial \log p(\mathbf{z}|\mathbf{x})}{\partial \mathbf{x}}\right)^T\right] = H^T R^{-1} H$$ $$\text{Var}(\hat{\mathbf{x}}) \geq \mathcal{I}(\mathbf{x})^{-1} \qquad \text{(CramΓ©r-Rao Bound)}$$

For a linear measurement $\mathbf{z} = H\mathbf{x} + \mathbf{v}$, $\mathbf{v} \sim \mathcal{N}(0, R)$: the FIM is $H^T R^{-1} H$. Adding a new sensor with measurement matrix $H_{new}$ and noise $R_{new}$ adds its FIM to the total: $\mathcal{I}_{total} = \mathcal{I}_{existing} + H_{new}^T R_{new}^{-1} H_{new}$. The CRB decreases (achievable accuracy improves) with every additional sensor. The Kalman filter achieves the CRB exactly for linear-Gaussian systems β€” it is the optimal linear estimator in this sense.

Sensor Contribution β€” Which Sensors Matter Most?

Worked Example 5.3 β€” FIM Analysis: IMU + Baro vs. IMU + Baro + Camera

A UAV navigates in 2D (x, z). Sensors: IMU provides velocity ($\sigma_v = 0.05$ m/s), barometer provides altitude ($\sigma_z = 2$ m). How much does adding a downward camera ($\sigma_{cam} = 0.1$ m position) improve achievable accuracy?

IMU + Baro FIM (velocity and altitude measurements): $$H_1 = \begin{bmatrix}0 & 1\end{bmatrix} \;(\text{velocity}), \quad H_2 = \begin{bmatrix}0 & 1\end{bmatrix} \;(\text{altitude in z})$$ $$\mathcal{I}_{1+2} = \frac{1}{0.05^2}\begin{bmatrix}0&0\\0&1\end{bmatrix} + \frac{1}{4.0}\begin{bmatrix}0&0\\0&1\end{bmatrix} = \begin{bmatrix}0 & 0\\0 & 400.25\end{bmatrix}$$ Horizontal (x) position: completely unobservable! $\mathcal{I}_{xx} = 0 \to$ CRB$_x = \infty$.

Adding downward camera ($H_3 = I_2$, observes both x and z): $$\mathcal{I}_{total} = \mathcal{I}_{1+2} + \frac{1}{0.01}\begin{bmatrix}1&0\\0&1\end{bmatrix} = \begin{bmatrix}100 & 0\\0 & 500.25\end{bmatrix}$$ Now: CRB$_x = 1/100 = 0.01$ mΒ² ($\sigma_x \geq 0.1$ m achievable). CRB$_z$ improves from $1/400.25 = 0.0025$ mΒ² to $1/500.25 = 0.002$ mΒ².

Conclusion: Without the camera, horizontal position is completely unobservable β€” it can drift without bound. Adding a downward camera changes the horizontal CRB from $\infty$ to 0.1 m. This is a qualitative improvement, not just quantitative β€” it converts an unobservable state into an observable one. This FIM analysis is exactly how military system designers justify the weight and power cost of each additional sensor.

5.8 Interactive: IMU + Camera + LiDAR Fusion Simulation

This simulation visualises a UAV fusing three sensor modalities simultaneously. The red/green/blue arrows show IMU acceleration vectors in the body frame β€” watch them shift as the UAV bobs in simulated flight. The amber cone is the camera frustum with feature detection rays radiating outward. The green point cloud and spinning beams are LiDAR returns and scan lines. The translucent sphere around the UAV is the EKF covariance ellipsoid β€” it should remain small when all sensors are active. Toggle each sensor off to see how uncertainty (ellipsoid size) grows.

β—ˆ IMU + CAMERA + LiDAR FUSION // M05 FUSED ERR: β€”
β€” β€” β€” β€” β€”

5.9 Python: Multi-Sensor Fusion with Chi-Square Fault Detection

What This Code Does Implements a multi-sensor EKF fusing IMU + barometer + simulated GPS/VIO, with chi-square innovation testing on every measurement update. Demonstrates: (1) correct covariance growth during IMU-only phases, (2) covariance reduction on measurement updates, (3) fault detection rejecting a spoofed GPS measurement at step 300. Run with: pip install numpy
Python
# multi_sensor_fusion.py
# ─────────────────────────────────────────────────────────────────
# Multi-sensor EKF with chi-square fault detection.
# Implements: MDPI Autonomous Control Ch.3, Bestaoui Sebbane Β§8.5–8.8
# Sensors: IMU (100 Hz) + Barometer (25 Hz) + GPS/VIO (1 Hz)
# Fault injection: GPS spoofing at t=3.0 s
# ─────────────────────────────────────────────────────────────────

import numpy as np
from scipy.stats import chi2

class MultiSensorEKF:
    """
    3-state vertical EKF for altitude + velocity + accel-bias fusion.
    State: x = [altitude_z, velocity_z, bias_az]
    Demonstrates full sensor fusion pipeline with fault detection.
    Bestaoui Sebbane Β§8.5
    """

    def __init__(self, dt_imu=0.01):
        self.dt   = dt_imu
        self.n    = 3

        # State: [altitude(m), vert_velocity(m/s), accel_bias(m/sΒ²)]
        self.x = np.array([0.0, 0.0, 0.0])

        # Initial covariance
        self.P = np.diag([25.0, 1.0, 1e-4])

        # Process noise Q β€” from IMU Allan variance (Worked Example 5.1)
        # sigma_a = 0.05 m/s²/√Hz (accelerometer noise density)
        # sigma_ba = 1e-4 m/s²/√Hz (accel bias random walk)
        sigma_a  = 0.05
        sigma_ba = 1e-4
        dt = self.dt
        self.Q = np.array([
            [sigma_a**2 * dt**3/3,  sigma_a**2 * dt**2/2, 0],
            [sigma_a**2 * dt**2/2,  sigma_a**2 * dt,       0],
            [0,                        0,                    sigma_ba**2 * dt],
        ])

        # Measurement noise matrices (Sadraey Β§6.2)
        self.R_baro = np.array([[4.0]])       # barometer: Οƒ=2m
        self.R_gps  = np.array([[25.0]])      # GPS altitude: Οƒ=5m

        # Chi-square fault detection threshold
        # alpha=0.001 (0.1% false alarm), m=1 measurement dimension
        self.chi2_threshold_1 = chi2.ppf(0.999, df=1)  # β‰ˆ10.83
        self.chi2_threshold_2 = chi2.ppf(0.999, df=2)  # β‰ˆ13.82

        # Diagnostics
        self.n_updates_accepted = 0
        self.n_updates_rejected = 0

    def predict(self, accel_meas: float):
        """
        IMU propagation β€” constant acceleration model with bias.
        F = βˆ‚f/βˆ‚x linearisation of kinematic equations.
        Bestaoui Sebbane Β§8.5 Eq. 8.11
        """
        z, vz, ba = self.x
        dt = self.dt

        # Bias-corrected vertical acceleration
        az_corr = accel_meas - ba

        # State propagation
        self.x = np.array([
            z  + vz*dt + 0.5*az_corr*dt**2,
            vz + az_corr*dt,
            ba,      # bias is random walk β€” no deterministic change
        ])

        # Jacobian F = βˆ‚f/βˆ‚x
        F = np.array([
            [1, dt, -0.5*dt**2],  # βˆ‚z_new/βˆ‚[z, vz, ba]
            [0,  1,       -dt  ],  # βˆ‚vz_new/βˆ‚[z, vz, ba]
            [0,  0,         1  ],  # βˆ‚ba_new/βˆ‚[z, vz, ba]
        ])

        # Covariance propagation
        self.P = F @ self.P @ F.T + self.Q

    def _chi2_gate(self, innovation, S, df) -> bool:
        """
        Chi-square innovation test β€” RAIM method.
        MDPI Autonomous Control Ch.3, Bestaoui Sebbane Β§8.8
        Returns True if measurement PASSES (should be accepted).
        """
        epsilon = float(innovation.T @ np.linalg.inv(S) @ innovation)
        threshold = self.chi2_threshold_1 if df == 1 else self.chi2_threshold_2
        return epsilon <= threshold, epsilon

    def _update(self, z_meas, H, R, sensor_name):
        """
        EKF measurement update with chi-square gating.
        Rejects measurements failing the chi-square test.
        """
        # Innovation
        innov = np.atleast_1d(z_meas - H @ self.x)

        # Innovation covariance
        S = H @ self.P @ H.T + R

        # Chi-square test BEFORE updating the filter
        passed, epsilon = self._chi2_gate(innov, S, df=len(innov))

        if not passed:
            self.n_updates_rejected += 1
            return False, epsilon   # REJECT β€” do not update filter

        # Kalman gain (Theorem 4.1 from Module 04)
        K = self.P @ H.T @ np.linalg.inv(S)

        # State update
        self.x = self.x + K @ innov

        # Covariance update β€” Joseph form (numerically stable)
        IKH = np.eye(self.n) - K @ H
        self.P = IKH @ self.P @ IKH.T + K @ R @ K.T

        self.n_updates_accepted += 1
        return True, epsilon   # ACCEPTED

    def update_baro(self, alt_meas: float):
        """Barometer altitude measurement update."""
        H = np.array([[1.0, 0.0, 0.0]])   # observes altitude directly
        return self._update(np.array([alt_meas]), H, self.R_baro, 'Baro')

    def update_gps_alt(self, alt_meas: float):
        """GPS altitude measurement update."""
        H = np.array([[1.0, 0.0, 0.0]])
        return self._update(np.array([alt_meas]), H, self.R_gps, 'GPS')


def run_fusion_demo():
    """
    Simulate 5-second altitude hold with GPS spoofing injection at t=3s.
    Demonstrates chi-square test rejecting the spoofed GPS fix.
    """
    dt   = 0.01    # 100 Hz IMU
    T    = 5.0
    N    = int(T / dt)
    ekf  = MultiSensorEKF(dt_imu=dt)

    TRUE_ALT   = 50.0   # true altitude [m]
    TRUE_BIAS  = 0.012  # true accel bias [m/sΒ²]
    SPOOF_TIME = 3.0    # GPS spoofing starts at 3s
    SPOOF_ALT  = 80.0   # spoofed altitude (30m above true)

    print(f"\n{'='*58}")
    print(f"  MULTI-SENSOR FUSION WITH FAULT DETECTION")
    print(f"  MDPI Autonomous Control Ch.3 | Bestaoui Sebbane Β§8.8")
    print(f"{'='*58}")
    print(f"  True altitude:  {TRUE_ALT} m")
    print(f"  IMU bias:       {TRUE_BIAS} m/sΒ² (simulated MEMS)")
    print(f"  GPS spoof at:   t={SPOOF_TIME}s β†’ false alt={SPOOF_ALT} m")
    print(f"  Chi-sq thresh:  {ekf.chi2_threshold_1:.2f} (Ξ±=0.1%, m=1)")
    print()
    print(f"  {'Time':>5} | {'True':>7} | {'Est':>7} | {'Err':>6} | {'P_zz':>6} | Event")
    print(f"  {'-'*62}")

    alt_error_history = []

    for k in range(N):
        t = k * dt

        # ── Simulate noisy IMU measurement ───────────────────────
        accel_meas = -TRUE_BIAS + np.random.randn() * 0.05  # gravity-subtracted vert accel

        # EKF predict step
        ekf.predict(accel_meas)

        event_str = ""

        # ── Barometer update at 25 Hz (every 4 IMU steps) ────────
        if k % 4 == 0:
            baro_meas = TRUE_ALT + np.random.randn() * 2.0
            passed, eps = ekf.update_baro(baro_meas)
            if not passed:
                event_str += f" [BARO FAULT Ξ΅={eps:.1f}]"

        # ── GPS update at 1 Hz (every 100 IMU steps) ─────────────
        if k % 100 == 0:
            # After SPOOF_TIME: inject false GPS altitude
            if t >= SPOOF_TIME:
                gps_alt = SPOOF_ALT + np.random.randn() * 5.0
                passed, eps = ekf.update_gps_alt(gps_alt)
                status = "ACCEPT" if passed else "REJECT"
                color  = "βœ“" if passed else "βœ—"
                event_str += f" [{color} GPS SPOOF Ξ΅={eps:.1f} {status}]"
            else:
                gps_alt = TRUE_ALT + np.random.randn() * 5.0
                passed, eps = ekf.update_gps_alt(gps_alt)
                event_str += f" [βœ“ GPS Ξ΅={eps:.2f}]"

        # Log and print at 1 Hz
        err = abs(ekf.x[0] - TRUE_ALT)
        alt_error_history.append(err)

        if k % 100 == 0:
            pzz = np.sqrt(ekf.P[0,0])
            print(f"  {t:5.1f}s | {TRUE_ALT:7.1f} | {ekf.x[0]:7.2f} | {err:6.2f}m | {pzz:6.3f}m |{event_str}")

    print(f"\n  Final bias estimate: {ekf.x[2]:.4f} m/sΒ²  (true: {TRUE_BIAS:.4f})")
    print(f"  Updates accepted:  {ekf.n_updates_accepted}")
    print(f"  Updates rejected:  {ekf.n_updates_rejected}  (spoofed GPS fixes)")
    mean_err = np.mean(alt_error_history)
    print(f"  Mean altitude error: {mean_err:.3f} m")
    print(f"\n  β†’ Chi-square test successfully rejected spoofed GPS.")
    print(f"  β†’ Navigation continued on barometer + IMU during attack.")
    print(f"{'='*58}\n")


if __name__ == "__main__":
    np.random.seed(7)
    run_fusion_demo()
Expected Output (key lines) At t=3.0s: [βœ— GPS SPOOF Ξ΅=384.6 REJECT] β€” chi-square test fires Β· At t=4.0s: [βœ— GPS SPOOF Ξ΅=392.1 REJECT] β€” repeated spoofed fixes rejected Β· Mean altitude error <2 m throughout (barometer+IMU fusion holds) Β· Final bias estimate converges to within ~10% of true bias Β· Updates rejected = 2 (exactly the two spoofed GPS fixes) Β· The navigation solution is never corrupted by the attack.
  1. A UAV uses loose coupling: a GPS-EKF and a VIO-EKF each produce independent position estimates, which are then fused in an outer navigation EKF. Explain carefully why this architecture produces an overconfident covariance estimate in the outer filter. Trace the mathematical mechanism through the innovation covariance equation $S = HPH^T + R$, and describe what happens when both the GPS-EKF and VIO-EKF simultaneously degrade due to a common-mode IMU error that the outer filter cannot detect.
  2. In Worked Example 5.2, the chi-square test had a threshold of 13.82 for $\alpha = 0.001$. A sophisticated GPS spoofer knows this threshold and designs their attack to inject a gradually drifting false position β€” moving the spoofed GPS solution by only 0.5 m per second. Calculate how many seconds the spoofer can drift before the chi-square test would detect the attack, assuming the UAV is approximately stationary ($P_{pos} = \text{diag}(4, 4)$ mΒ², $R_{GPS} = \text{diag}(25, 25)$ mΒ²). What does this imply about the limitations of chi-square RAIM against slow spoofing?
  3. Using the Fisher Information Matrix analysis from section 5.7, prove that adding a downward-facing camera at a non-zero lever arm from the IMU provides more navigational information than a co-located camera at the same location as the IMU. Specifically, show how the lever arm coupling term in the measurement Jacobian contributes to the FIM, and what physical quantity it makes observable that would otherwise be unobservable from a purely co-located sensor pair.
β—‚ Module 04: GNSS-Denied Nav Module 06: ML Foundations β–Έ