- Where the robot really is
- Where the robot thinks it is
- Raw sensor readings
- Slippery corridor: landmarks and GPS drop out
In plain words Why this matters
Imagine walking through a pitch-black warehouse. You count your steps (odometry), but each step introduces drift — after 50 meters, you could be several meters off course. Every time you reach out and touch a wall (sensor update), you correct your mental map.
The Extended Kalman Filter does exactly this for autonomous vehicles: it continuously weighs the robot's internal motion estimate against external sensor readings. When tuned correctly, it ensures the robot always knows where it is — even when the floor is slippery or visual landmarks disappear.
Business outcome: Reliable navigation without expensive high-precision sensors. Lower costs, higher uptime, safer operations.
Jargon buster
- AGV
- Automated Guided Vehicle: a robot that moves goods around a warehouse.
- EKF
- Extended Kalman Filter: the "brain" that blends all sensor readings into one best guess of the robot's position.
- Odometry
- Counting wheel turns. Cheap, but it drifts, and fails on slippery floors.
- IMU
- A motion sensor (like the one in your phone) that feels turns and acceleration.
- LIDAR
- A laser scanner that measures the distance to landmarks and beacons.
- Beacon
- A cheap marker you place on the map so LIDAR has something to measure against.
- GPS
- Satellite positioning. Rough indoors, and it drops out in the corridor.
Hardware Savings Calculator
Formula & sensor costs
Higher EKF accuracy → lower $\sigma$ → higher perfFactor → more savings.
Removing sensors reduces BOM baseline and absolute savings.
Business Impact
Reduced Deployment Risk: The "slippery floor" scenario simulates real-world odometry failure (slick surfaces, uneven terrain). A well-tuned EKF prevents catastrophic divergence.
Lower BOM Costs: With optimal sensor fusion, you can use lower-spec lidars/cameras while maintaining reliability — reducing per-unit cost by up to 40%.
Minimized Downtime: Real-time state recovery means the AGV handles edge cases without human intervention, keeping logistics lines running.
For engineers The Predict-Update Loop & Covariance Tuning
The EKF operates in two phases per timestep:
1. Predict (Motion Model): The state x is advanced via a non-linear
kinematic
function f(x, u). The covariance P grows because the motion model has
uncertainty
— this is encoded in the Process Noise matrix Q.
2. Update (Sensor Fusion): A noisy measurement z arrives. The filter
computes
the Kalman Gain K = P·Hᵀ(H·P·Hᵀ + R)⁻¹, which optimally weights the prediction vs. the
measurement. The Measurement Noise matrix R determines how much the filter trusts the
sensor.
The Q and R matrices are your confidence dials:
— Low Q + high R = "trust the motion model, ignore noisy sensors"
— High Q + low R = "trust the sensors, the motion model is unreliable"
A balanced tuning (Q ≈ R in the right units) allows the filter to dynamically adapt — trusting the model when sensors fail, and trusting sensors when the model drifts.
The math EKF Mathematics & Jacobian Linearization
State Vector (4 DOF): planar pose + velocity
Non-Linear Motion Model
Jacobian of f w.r.t. state (linearization point: current estimate)
Predict Step
Measurement Model (range & bearing to landmark at (lx, ly))
Jacobian of h w.r.t. state
where $\Delta x = l_x - x$, $\Delta y = l_y - y$, $d = \sqrt{{\Delta x}^2 + {\Delta y}^2}$
Kalman Gain & Update Step
Sensor Noise Models (accuracy $a_{\text{sensor}} \in [0, 50]$)
Each $a$ is the slider value for that sensor. Higher accuracy → smaller $\sigma$ → the filter trusts that sensor more. At $a = 0$ the noise is infinite (sensor ignored); at $a = 50$ the noise is effectively zero. The beacon accuracy $a_{\text{beacon}}$ is set per-beacon in the list below the LIDAR range slider. Internally the EKF packs these $\sigma$ values into the noise covariance matrices $\mathbf{Q}$ (process) and $\mathbf{R}$ (measurement). The IMU $\sigma_{\text{imu}}$ scales the process noise $\mathbf{Q}_{\text{imu}}$ used in the IMU-driven dead-reckoning step.
Key Insight: Because both f and h are non-linear, we linearize them via their Jacobians at each timestep — this is what makes the filter Extended. The Jacobians capture the local slope of the non-linear functions, enabling approximately optimal fusion of non-linear models and measurements.
Symbol legend
Live Metrics
Status shows the simulation state.
Uncertainty is the trace of the
EKF's
position covariance — higher means the filter is less confident about where the AGV is.
EKF Divergence is the Euclidean distance between the EKF estimate and the true AGV
position;
if it exceeds 60 px the filter has lost track.
Sensors Configuration
Interior Sensors
Show the math
Show the math
Exterior Sensors
Show the math
Click +Add, then click the map. Drag to move. ✕ to remove.
Show the math
Route Configuration
Drag the A and B markers on the map to reposition the start and end points.
Corridor Scenario
Drag the corridor markers along the path on the map to set the featureless zone.