State Estimation

Negative sign: if the robot turns more (increases θ), the x-component of its motion decreases (the robot moves less in x and more in y). At θ=30°, this sensitivity is −0.1 meters per radian of heading change.

Positive: turning more (increasing θ) makes the robot move more in the y-direction at this heading. At 45°, the sensitivity is symmetric between x and y — both partial derivatives have the same magnitude (1.061).

The 1 comes from the linear term x (its derivative is 1). The 2x·dt comes from the quadratic term. At x=0, the Jacobian is 1 — the system looks linear. As |x| grows, the Jacobian deviates from 1, meaning the linearization captures less of the true dynamics.

javascript
function robotJacobian(theta, v, dt) {
  return [
    [1, 0, -v * Math.sin(theta) * dt],
    [0, 1,  v * Math.cos(theta) * dt],
    [0, 0,  1]
  ];
}

Line 5 is the bug. The covariance predict formula is P̄ = F · P · F^T + Q. The code computes F · P · F instead of F · P · F^T. When F is not symmetric (it rarely is), multiplying by F instead of F^T gives a non-symmetric P̄, which is not a valid covariance matrix. The result grows without bound because the eigenvalues of FPF can be larger than those of FPF^T.

The landmark is roughly 53 degrees to the left of the robot's forward direction (which is along the x-axis when θ=0).

Negative because if the robot moves in +x (Δx decreases), range decreases. The magnitude 0.6 says: 1 meter of robot x-motion changes the range by 0.6 meters. This makes sense — the landmark is not directly in front of the robot, so x-motion only partially closes the gap.

The bearing row has much smaller magnitudes (0.16, 0.12) than the range row (0.6, 0.8) because bearing changes slowly with position but directly with heading (−1 in the θ column).

The range residual is +0.2m: the measurement says the landmark is slightly farther than we predicted. The bearing residual is +0.023 rad (~1.3°): the landmark appears slightly more to the left than expected. Both are small, suggesting our prediction is good.

javascript
function rangeBearingH(rx, ry, rtheta, lx, ly) {
  const dx = lx - rx, dy = ly - ry;
  const r2 = dx*dx + dy*dy;
  const r  = Math.sqrt(r2);
  return [
    [-dx/r, -dy/r, 0],
    [dy/r2, -dx/r2, -1]
  ];
}

The sigma points form a cross pattern: the center point plus 4 points along the principal axes of the covariance ellipse. More variance in x (=4) means the x-axis points are spread further apart (3.464 from center) than the y-axis points (1.732).

The true mean of x² when x~N(1,4) is E[x²] = Var(x) + E[x]² = 4 + 1 = 5. The UKF gets this exactly — because x² is a second-order polynomial, and sigma points capture the first two moments perfectly. An EKF would get this wrong (it would predict 1² = 1 and miss the variance term).

javascript
function sigmaPoints(mu, P, kappa) {
  var n = 2;
  var s = n + kappa;
  var s0 = Math.sqrt(s * P[0][0]);
  var s1 = Math.sqrt(s * P[1][1]);
  return [
    [mu[0], mu[1]],
    [mu[0]+s0, mu[1]],
    [mu[0], mu[1]+s1],
    [mu[0]-s0, mu[1]],
    [mu[0], mu[1]-s1]
  ];
}

The EKF misses the σ²·dt = 0.1 term because linearization throws away the curvature (second derivative) information. The UKF captures this term automatically through sigma point propagation. The error is σ² × d²f/dx² × dt / 2 — it grows with both uncertainty and curvature.

The EKF-UKF gap is proportional to σ² × curvature. It matters most when uncertainty is large AND the dynamics are nonlinear. GPS dropout causes large σ²; re-acquiring a target involves highly nonlinear bearing/range measurements. The other scenarios either have small σ² (precise sensors) or linear dynamics (straight road, temperature).

13 function evaluations vs. 36 partial derivatives. The UKF is often computationally cheaper because function evaluations are simpler than analytical derivatives. And you never need to derive the Jacobian symbolically — just evaluate f(x) at 13 points. For systems where the Jacobian is hard to derive (or impossible, like neural network dynamics), the UKF wins by default.

First assess nonlinearity (how curved are the dynamics?). Then check if the Jacobian is tractable. Then test both with real noise levels. Finally choose based on results. Many practitioners start with EKF because it's simpler, then switch to UKF only if linearization error causes problems. The UKF has no downside except slightly more code complexity and Cholesky decomposition overhead.

This particle is 1 unit from the measurement. The (z−x)²/8 denominator comes from 2σ² where σ²=4. The closer a particle is to the measurement, the higher its weight.

Particles at x=3 and x=5 get equal weight (both are 1 unit from z=4). Together they hold 72% of the total weight. The particle at x=9 is 5 units away and gets only 1.8% — it's practically dead.

Out of 5 particles, only ~3.4 are "effective." If all weights were equal (0.2 each), N_eff would be 5. The closer N_eff is to 1, the more degenerate the particle set. A common rule: resample when N_eff < N/2.

Thresholds: 0.1, 0.3, 0.5, 0.7, 0.9. Cumulative ranges: particle 1=[0, 0.132], particle 2=(0.132, 0.491], particle 3=(0.491, 0.850], particle 4=(0.850, 0.982], particle 5=(0.982, 1.0].

0.1 → particle 1, 0.3 → particle 2, 0.5 → particle 3, 0.7 → particle 3, 0.9 → particle 4. Particle 2 is selected once. Particle 3 gets two copies (it had weight 0.359). Particle 5 (weight 0.018) gets zero copies — it's been killed by resampling.

javascript
function particleWeights(positions, z, sigma2) {
  const raw = positions.map(x =>
    Math.exp(-(z - x) * (z - x) / (2 * sigma2))
  );
  const sum = raw.reduce((a, b) => a + b, 0);
  return raw.map(w => w / sum);
}

N_eff = 1.06 out of 5 — catastrophic degeneracy. One particle has 97% of the weight. You absolutely must resample. After resampling, you'd have ~5 copies of the dominant particle (with slight perturbation) and zero copies of the others. This is why particle filters need hundreds or thousands of particles for real problems.

With no prior, the posterior is just the measurement distribution. After multiple GPS readings, the variance decreases, but never below the measurement noise floor (since each reading is independent).

The fused estimate is closer to the IMU reading (102) because the IMU has lower variance (9 vs 25). The fusion trusts the more precise sensor more. The fused variance is 1/(1/25 + 1/9) = 25×9/34 = 6.62 m² (σ = 2.57m) — better than either sensor alone.

The fused uncertainty (2.57m) is better than both GPS (5m) and IMU (3m). This is the fundamental benefit of sensor fusion: combining independent measurements always reduces uncertainty. The fused variance is always less than the smallest individual variance.

[x, y, vx, vy, θ] is the minimum. GPS directly observes [x, y]. IMU acceleration integrates to velocity, so we need [vx, vy] in the state to fuse IMU. Magnetometer observes θ directly. We don't need acceleration in the state because we can treat it as a control input rather than a state variable (a common design choice). However, if you want to estimate IMU bias, you'd add [b_ax, b_ay] for a 7-element state.

A tiny 0.01 m/s² bias causes 18 meters of drift in just one minute. Drift grows quadratically with time — at 10 minutes it would be 1800m. This is why IMU-only navigation fails quickly and GPS corrections are essential. It's also why estimating the bias as part of the state vector is critical for any production system.

Rank 2 = n, so the system is fully observable. Even though we only measure position, the dynamics couple position and velocity. By observing how position changes over time, we can infer velocity. This is exactly what the Kalman filter does — the off-diagonal terms in P capture this position-velocity coupling.

Not observable! Measuring velocity alone cannot determine position, because in the constant-velocity model, velocity doesn't depend on position. The velocity measurement gives us v directly, but there's no way to figure out x from repeated v measurements — x could be any value. Physically: knowing how fast you're going doesn't tell you where you are.

If F = [[1,1],[0.1,0.9]], then H·F = [0,1]·[[1,1],[0.1,0.9]] = [0.1, 0.9]. Now O = [[0,1],[0.1,0.9]], which has det = −0.1 ≠ 0 → rank 2. The system is observable because velocity now depends on position (drag/spring force), so observing velocity changes reveals position information. Measurement rate and noise affect estimation quality, not observability itself — observability is a structural property of (F, H).

O(2,3) = 0.5. The full O has rank 3 (det = 1×(1×2 − 0.5×2) − 0×(...) + 0×(...) = 1), so the system is fully observable. Position measurements alone can recover velocity and acceleration, because F couples them.

javascript
function obsMatrix(H, F) {
  const HF = [
    H[0]*F[0][0] + H[1]*F[1][0],
    H[0]*F[0][1] + H[1]*F[1][1]
  ];
  return [[H[0], H[1]], [HF[0], HF[1]]];
}

Small Q tells the filter "the model is very reliable, almost no process noise." So K becomes small (trust model, ignore measurements). When a sudden jump happens, the filter's prediction is now far from the truth, but it stubbornly trusts its (wrong) model and only slowly incorporates the new measurements. This manifests as lag — the estimate trails behind reality.

K ≈ 0.39: the filter gives 39% weight to measurements. With Q/R = 0.25, the model is moderately trusted. If we made Q=4, R=1 (Q/R=4), we'd get K ≈ 0.78 — heavily trusting measurements.

Tiny R tells the filter "the sensor is perfect." So K → 1, and the estimate snaps to every raw measurement. Since the true noise is σ=2m, the output jitters wildly. This is worse than just using raw measurements, because the filter also claims very high confidence (small P) while being noisy — it's both wrong and overconfident.

0.618 is the golden ratio minus 1. When Q=R, the filter splits trust roughly 62/38 toward measurements. For Q=0.01: P≈0.099, K≈0.09 (model wins). For Q=100: P≈10.5, K≈0.91 (sensor wins). The Q/R ratio is the master tuning knob.

Line 6 is the bug. The update step P = (1−KH)P should NOT add Q again. Q is already added in the predict step (line 2). Adding it a second time inflates P, which at steady state forces a larger P but also means line 2 of the next iteration adds Q on top of an already-too-large P. The net effect is that S in line 3 becomes larger, making K smaller than expected. The fix: remove the "+ Q" from line 6.

Large innovations mean the predictions are worse than the filter thinks. This could mean either Q is too small (model isn't as reliable as assumed) or R is too small (sensor noise is underestimated). Increasing Q is the safer choice because it makes the filter more responsive without making it overfit to noise. In practice, the innovation covariance ∑(y·y^T)/N should approximately equal S. If it's consistently larger, Q should increase.

9 base states (pos, vel, attitude) + 3 accelerometer biases + 3 gyroscope biases = 15. The biases are slowly time-varying, modeled as random walks in the state transition. By estimating them online, the filter can subtract the bias from future IMU readings, dramatically reducing drift. Some systems also add 3 magnetometer biases for 18 total states.

At 100Hz, each predict step moves only 0.1m. This tiny step is why IMU integration is smooth — each step is essentially linear. The nonlinearity accumulates over many steps, which is where drift comes from.

When stationary, the IMU reads +g upward (it measures the reaction force against gravity). Adding the gravity compensation g = [0,0,−9.81] cancels this out, giving zero net acceleration. This is the most common source of bugs in INS code: forgetting to subtract gravity, causing the filter to think the vehicle is accelerating upward at 9.81 m/s².

K = 0.074: the filter barely moves toward the GPS reading. Why? Because the IMU-propagated uncertainty (P=2 m², σ=1.4m) is much smaller than GPS noise (R=25 m², σ=5m). After only 1 second, the IMU is still more trustworthy. After 60 seconds without GPS, P would grow to ~324 m² (σ=18m), and K would jump to ~0.93. This is the GPS/IMU handoff in action.

360 meters of drift in 2 minutes from a 0.05 m/s² bias. This is why GPS outages are critical for autonomous vehicles. Solutions: (1) estimate and subtract the bias (the 15-state filter), (2) use additional absolute position sources (LiDAR, visual odometry), (3) use a higher-quality IMU with smaller bias (tactical grade: 0.001 m/s², giving only 7.2m drift in 2 min).

120 values to track, and FPF^T involves 15×15 matrix multiplications at 100Hz. That's ~675,000 multiply-adds per second just for covariance propagation. This is why embedded INS systems use the square-root form (Cholesky) or the Joseph form for numerical stability — and why the UKF with 31 sigma points is sometimes preferred over the EKF for this problem.

1. Read IMU — get latest accelerometer and gyroscope measurements. 2. EKF predict — propagate state and covariance forward using IMU data. 3. Check GPS — GPS arrives at 1Hz, so 99 out of 100 cycles have no GPS. 4. GPS update (if available) — correct position using GPS measurement. 5. Output state — publish the current best estimate to downstream systems. The predict step runs every cycle (100Hz), but the GPS update only fires ~once per second.