⚡ The EKF Tuner

Can you get a warehouse robot across a slippery corridor where its sensors go blind?

An interactive demo of sensor fusion (Extended Kalman Filter) — no math needed to play.

View on GitHub

  1. 1Pick the robot's sensors in the control panel, under Sensors Configuration. Better sensors cost more.
  2. 2Press Run and watch the robot drive from A to B through the slippery corridor.
  3. 3Win if it reaches B without hitting the shelves. The calculator shows how much hardware money your setup saves.
  • 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

Sensor Cost Reduction 0%
Savings = EKF performance (0–40% based on position covariance) applied to your selected sensor BOM. Tune the filter well and choose cost-effective sensors to maximize savings.
Formula & sensor costs
$$\text{perfFactor} = 0.40 \times \max\!\big(0,\; 1 - \frac{\sigma}{3.0}\big)$$
$$\text{BOM} = C_{\text{wheel}} + C_{\text{lidar}} + C_{\text{gps}} + C_{\text{imu}} + n_{\text{beacon}} \times C_{\text{beacon}}$$
$$\text{savings} = \text{round}(\text{perfFactor} \times \text{BOM})$$
Sensor costs: wheel $200 · lidar $5,000 · gps $800 · imu $3,000 · beacon $150 ea.
Where $\sigma = \sqrt{\text{tr}(P_{[0:2,0:2]})}$ is the position standard deviation from the EKF covariance.
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

$$ \mathbf{x}_k = \begin{bmatrix} x \\ y \\ \theta \\ v \end{bmatrix} $$

Non-Linear Motion Model

$$ \mathbf{x}_{k+1} = f(\mathbf{x}_k, \mathbf{u}_k) = \begin{bmatrix} x_k + v_k \cos(\theta_k) \Delta t \\ y_k + v_k \sin(\theta_k) \Delta t \\ \theta_k + \dot{\theta} \Delta t \\ v_k + \dot{v} \Delta t \end{bmatrix} $$

Jacobian of f w.r.t. state (linearization point: current estimate)

$$ \mathbf{F}_k = \frac{\partial f}{\partial \mathbf{x}} \bigg|_{\mathbf{x}_{k-1}, \mathbf{u}_k} = \begin{bmatrix} 1 & 0 & -v_k \sin(\theta_k) \Delta t & \cos(\theta_k) \Delta t \\ 0 & 1 & v_k \cos(\theta_k) \Delta t & \sin(\theta_k) \Delta t \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix} $$

Predict Step

$$ \mathbf{x}_k^- = f(\mathbf{x}_{k-1}, \mathbf{u}_k) $$
$$ \mathbf{P}_k^- = \mathbf{F}_k \mathbf{P}_{k-1} \mathbf{F}_k^\mathsf{T} + \mathbf{Q} $$

Measurement Model (range & bearing to landmark at (lx, ly))

$$ \mathbf{z}_k = h(\mathbf{x}_k) = \begin{bmatrix} \sqrt{(l_x - x_k)^2 + (l_y - y_k)^2} \\ \text{atan2}(l_y - y_k,\; l_x - x_k) - \theta_k \end{bmatrix} $$

Jacobian of h w.r.t. state

$$ \mathbf{H}_k = \frac{\partial h}{\partial \mathbf{x}} \bigg|_{\mathbf{x}_k^-} = \begin{bmatrix} -\frac{\Delta x}{d} & -\frac{\Delta y}{d} & 0 & 0 \\ \frac{\Delta y}{d^2} & -\frac{\Delta x}{d^2} & -1 & 0 \end{bmatrix} $$

where $\Delta x = l_x - x$, $\Delta y = l_y - y$, $d = \sqrt{{\Delta x}^2 + {\Delta y}^2}$

Kalman Gain & Update Step

$$ \mathbf{K}_k = \mathbf{P}_k^- \mathbf{H}_k^\mathsf{T} \left( \mathbf{H}_k \mathbf{P}_k^- \mathbf{H}_k^\mathsf{T} + \mathbf{R} \right)^{-1} $$
$$ \mathbf{x}_k^+ = \mathbf{x}_k^- + \mathbf{K}_k \left( \mathbf{z}_k - h(\mathbf{x}_k^-) \right) $$
$$ \mathbf{P}_k^+ = \left( \mathbf{I} - \mathbf{K}_k \mathbf{H}_k \right) \mathbf{P}_k^- $$

Sensor Noise Models (accuracy $a_{\text{sensor}} \in [0, 50]$)

$$ \sigma_{\text{odom}} = \frac{1}{a_{\text{odom}}}, \qquad \sigma_{\text{lidar}} = \frac{1}{a_{\text{lidar}}}, \qquad \sigma_{\text{gps}} = \frac{1}{a_{\text{gps}}} $$
$$ \sigma_{\text{imu}} = \frac{1}{a_{\text{imu}}}, \qquad \sigma_{\text{beacon}} = \frac{1}{a_{\text{beacon}}} $$

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
$x$ State vector $[x, y, \theta, v]^\mathsf{T}$ $u$ Control input $[\dot\theta, \dot v]^\mathsf{T}$ $f$ Non-linear motion model $h$ Non-linear measurement model (range & bearing) $F$ Jacobian of $f$ w.r.t. state $H$ Jacobian of $h$ w.r.t. state $P$ State covariance matrix $Q$ Process noise covariance (odometry uncertainty) $R$ Measurement noise covariance (sensor uncertainty) $K$ Kalman gain — weights prediction vs. measurement $I$ Identity matrix $z$ Measurement vector from sensor $\Delta t$ Simulation timestep $l_x, l_y$ Landmark position $d$ Euclidean distance to landmark $\theta$ Heading (yaw) angle $v$ Linear velocity
Faster speed increases the distance traveled per timestep, making the corridor harder to navigate.
A start (150, 400)  ·  B end (700, 160)
60 px
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.

Status Awaiting Start
Position Uncertainty (tr(P)) —
EKF Divergence —
Sensors Configuration

Interior Sensors

1.00 ⓘ
Show the math
Accuracy $a_{\text{odom}}$ → $\sigma_{\text{odom}} = 1 / a_{\text{odom}}$.
Higher = lower process noise std dev = filter trusts wheel encoders more.
At $a_{\text{odom}} = 0$, $\sigma_{\text{odom}}$ is infinite (filter ignores odometry).
At $a_{\text{odom}} = 50$, $\sigma_{\text{odom}} \approx 0$ (near-perfect odometry).
1.00 ⓘ
Show the math
Accuracy $a_{\text{imu}}$ → $\sigma_{\text{imu}} = 1 / a_{\text{imu}}$.
IMU measures heading ($\theta$) via gyroscope and velocity ($v$) via accelerometer,
with noise $\propto \sigma_{\text{imu}}$. These are superimposed as a measurement update
on top of the odometry prediction (both contribute simultaneously).
Higher $a_{\text{imu}}$ = lower $\sigma_{\text{imu}}$ = filter trusts IMU measurements more.
At $a_{\text{imu}} = 0$, $\sigma_{\text{imu}}$ is infinite (filter ignores IMU).
At $a_{\text{imu}} = 50$, $\sigma_{\text{imu}} \approx 0$ (near-perfect heading & velocity readings).

Exterior Sensors

1.00 ⓘ
Show the math
Accuracy $a_{\text{lidar}}$ → $\sigma_{\text{lidar}} = 1 / a_{\text{lidar}}$.
Higher = lower measurement noise std dev = filter trusts LIDAR more.
At $a_{\text{lidar}} = 0$, $\sigma_{\text{lidar}}$ is infinite (filter ignores LIDAR).
At $a_{\text{lidar}} = 50$, $\sigma_{\text{lidar}} \approx 0$ (near-perfect detections).
Range 400
Beacons (detected by LIDAR)

Click +Add, then click the map. Drag to move. ✕ to remove.

1.00 ⓘ
Show the math
Accuracy $a_{\text{gps}}$ → $\sigma_{\text{gps}} = 1 / a_{\text{gps}}$.
Higher = lower position std dev = filter trusts GPS more.
At $a_{\text{gps}} = 0$, $\sigma_{\text{gps}}$ is infinite (filter ignores GPS)
At $a_{\text{gps}} = 50$, $\sigma_{\text{gps}} \approx 0$ (near-perfect fixes).
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.