← Back to the chapter · Previous detail · Next detail →
Technical detail · IoT Mathematics: Fusion and Sampling Decisions
Repeated prediction and GPS correction
Step 5: Real-World Performance Calculation
For a delivery truck moving at 60 km/h (16.67 m/s):
- GPS update rate: 1 Hz (once per second)
- Dead reckoning uncertainty growth: 2% per second (from accelerometer bias)
Between GPS updates (1 second):
- Distance traveled: 16.67 meters
- Dead reckoning error: 16.67 x 0.02 = 0.33m (grows linearly)
- Fused uncertainty after 1 sec:
After GPS update:
- Pre-update: sigma = 4.48m
- Post-fusion: sigma ≈ 4.09m (the new GPS update reduces uncertainty below its pre-update value)
Key Insight: Kalman filter prevents dead reckoning drift by periodically re-anchoring position with GPS, while smoothing GPS noise with prediction.
Implementation Pseudocode:
# Initialize state
state = [position_x, position_y, velocity_x, velocity_y]
P = [[25, 0, 0, 0], # Position covariance matrix (5m uncertainty)
[0, 25, 0, 0],
[0, 0, 4, 0], # Velocity covariance (2 m/s uncertainty)
[0, 0, 0, 4]]
# Prediction step (every 0.1 seconds)
state_predicted = F @ state # Matrix multiplication from Section C.3
P_predicted = F @ P @ F.T + Q # Add process noise Q
# Measurement step (GPS reading every 1 second)
innovation = gps_reading - H @ state_predicted # H extracts position
S = H @ P_predicted @ H.T + R # Innovation covariance (R = GPS noise)
K = P_predicted @ H.T @ inv(S) # Kalman gain (optimal weighting)
state = state_predicted + K @ innovation # Update state
P = (I - K @ H) @ P_predicted # Update covariance
Math concepts used: Vectors (C.1), matrices (C.2), matrix multiplication (C.3), Gaussian distribution (D.2), sensor fusion (D.3), integrals (A.2).