Lecture 7 — State Estimation and Fusion (II)

Legend
each Gaussian — where the robot may be: the likeliest distance at its centre, less likely to the sides
$\hat d$ — the estimate of the distance to the wall, the green centre; the hat — an estimate; dashed green — the last one
$\sigma_{\hat d}$ — its typical error, the width; $\sigma^2$ — its variance, the doubt
$\textcolor{#1f77b4}{d^-}$, $\sigma_{d^-}$ — the prediction and its error; the minus — before the scan
$v\,\Delta t$ — one step by the wheel odometry: speed × time
$\sigma_u$ — the error one step adds
$\textcolor{#d62728}{z}$ — the scan of the lidar's forward ray; $\sigma_z$ — its error: the lidar's own, constant
$K$ — the Kalman gain: the scan's weight, from 0 to 1
$z - d^-$ — the innovation: how far the scan is from the prediction

Legend
$\hat d$ — the estimate of the distance to the wall after the last scan, before the robot moves; $\sigma_{\hat d}$ — its typical error, 1 cm (example)
$v\,\Delta t$ — the step: how far the wheel odometry says the robot moved between two scans
$\sigma_u$ — the error one step adds: the wheels', 2 cm (example); $\sigma_u^2$ — Q in filterpy and robot_localization
$\textcolor{#1f77b4}{d^-}$, $\textcolor{#1f77b4}{\sigma_{d^-}}$ — the prediction and its error; the minus — before the scan
$\sigma^2$ — a variance; variances add, σ do not: 1 + 4 = 5, so $\sigma_{d^-}$ = √5, not 1 + 2
mean1, var1 — the estimate: $\hat d$, $\sigma_{\hat d}^2$; mean2, var2 — the step: $-v\,\Delta t$, $\sigma_u^2$
green — the estimate; dashed — the step's error; blue — the prediction

Legend
$d$ — the true distance to the wall
$\textcolor{#1f77b4}{d^-}$ — the prediction: the last estimate moved by the wheel odometry's step; the minus — before the scan
$\sigma_{d^-}$ — the prediction's typical error
$\hat d_{k-1}$ — the estimate one step earlier, at step $k-1$
$\textcolor{#d62728}{z}$ — the lidar's forward ray; $\sigma_z$ — its typical error
$\textcolor{#2ca02c}{\hat d}$ — the mix, the fused estimate: the new estimate
$v\,\Delta t$ — how far the robot moved in one step: speed × time
$w$ — the scan's weight: any number from 0 to 1. The loop's $K$ is the best $w$ — found next
blue, red — where the wall may be, by the wheel odometry and by the lidar: likeliest at the centre; dotted line — where it truly is; dashed green — the mix, not known yet

Legend
$w$ — the scan's weight: here 0.8 for the lidar, 0.2 for the wheels
lidar, wheels — the error of each, cm; every sign equally likely
mix — 0.8 × the lidar's error + 0.2 × the wheels' error
mean square — the average of the error²
$\sigma_{\hat d}$ — the mix's typical error: the root of the mean square
lidar alone: $\sigma_z$ = 1 cm · wheels alone: $\sigma_{d^-}$ = 2 cm (example)
green curve — $\sigma_{\hat d}$ for every weight; red dashed — the lidar alone; shaded — where the mix beats it

Legend
$\textcolor{#2ca02c}{\sigma_{\hat d}^2}(w)$ — the mix's variance, its doubt: the average of its error² over many scans, cm²
$w$ — the scan's weight, from 0 to 1
$\textcolor{#1f77b4}{\sigma_{d^-}^2}$ — the prediction's variance: the average of its error²
$\textcolor{#d62728}{\sigma_z^2}$ — the lidar's variance
$(1-w)^2 \sigma_{d^-}^2$ — the prediction's part: its error × (1 − w), so its variance × (1 − w)²: blue dashed
$w^2 \sigma_z^2$ — the lidar's part: its error × $w$, its variance × $w^2$: red dashed
green — their sum, $\sigma_{\hat d}^2$; dots — a few weights, with their values

Legend
green curve — the mix's variance $\sigma_{\hat d}^2$ for every weight $w$ of the scan; green dot — its bottom, the best $w$
dashed line — from the bottom down to the bar: the bar splits right under it
bar — the total doubt $\sigma_{d^-}^2 + \sigma_z^2$: blue — the prediction's part, red — the lidar's
$K$ — the Kalman gain: the best weight of the scan, from 0 to 1
$\textcolor{#1f77b4}{\sigma_{d^-}^2}$ — the prediction's variance: its typical error, squared
$\textcolor{#d62728}{\sigma_z^2}$ — the lidar's variance
$\sigma_{\hat d}^2$ — the mix's variance, its doubt, cm²
$\sigma_{d^-} = 0$ → $K = 0$: keep the prediction · $\sigma_z = 0$ → $K = 1$: take the scan

Legend
$z$ — the reading: the lidar's distance, the IMU's yaw
$H$ — the measurement matrix: picks from the state what the sensor reads; $n$ — state numbers, $m$ — readings
$\textcolor{#1f77b4}{P^-}$, $\textcolor{#d62728}{R}$ — the prediction's and the sensor's covariance matrices
$a$ — a number an error is multiplied by; $\sigma^2$ — that error's variance
$H^\top$ — $H$ turned over; $S$ — the reading's whole doubt; $S^{-1}$ — the inverse of $S$; $K$ — the Kalman gain
grey — $H$'s ones; framed — what they pick

Legend
$H$ — picks from the state what the sensor reads; $H^\top$ — the same ones as a column
$\textcolor{#1f77b4}{P^-}$ — the prediction's covariance matrix: variances on the diagonal, blue
cov — a covariance: how two errors depend on each other
$\sigma^2_x$, $\sigma^2_y$, $\sigma^2_\theta$ — the variances of $x$, $y$ and the yaw $\theta$; $\sigma^2_{d^-}$ — our prediction's
$a$ — a number an error is multiplied by; $\sigma^2$ — its variance
framed — what the ones pick; dashed — cov$(y, \theta)$

Legend
$K$ — the Kalman gain; $H$ — picks what the sensor reads
$\textcolor{#1f77b4}{P^-}$, $\textcolor{#d62728}{R}$ — the prediction's and the sensor's covariance matrices
$S$ — the reading's whole doubt
bar — $S$ (example): blue — the prediction's part, red — the lidar's; under the blue part — $K$

Linear algebra
$A\,B$ — each row of $A$ times each column of $B$, summed; $(m \times n)(n \times p) = m \times p$
$A^\top$ — rows become columns
$A^{-1}$ — the inverse: $A^{-1}A = I$, the identity; for a number, $1/A$

Legend
blue — the prediction's part, red — the lidar's
$K$ — the Kalman gain, the scan's weight: self.K
$\textcolor{#1f77b4}{P^-}$ — the prediction's doubt, our $\sigma_{d^-}^2$: self.P
$\textcolor{#d62728}{R}$ — the lidar's doubt, our $\sigma_z^2$: R
$H$ — picks what the sensor reads: 1, the distance itself
$H^\top$ — $H$ turned over: H.T
PHT — $P^-H^\top$, computed once for speed
$S$ — the whole doubt: self.S; SI — $S^{-1}$
dot — the matrix product: rows of the left times columns of the right

Legend
$\textcolor{#1f77b4}{\sigma_{d^-}}$ — the prediction's doubt as a typical error: the upper slider
$\sigma_{\hat d}$ — the estimate's typical error after the last scan
$\sigma_u$ — what one step of the wheel odometry adds: its speed error × the time between scans
$\textcolor{#d62728}{\sigma_z}$ — the lidar's typical error, measured standing: the lower slider
$w$ — the scan's weight; $K$ — the best $w$, the bottom of the curve
$\sigma^2_{\hat d}(w)$ — the estimate's variance for each $w$
start (example): $\sigma_{d^-}$ = 2 cm, $\sigma_z$ = 1 cm

Legend
blue — the prediction, red — the forward ray, green — the estimate
$\textcolor{#2ca02c}{\hat d}$, $\textcolor{#2ca02c}{\sigma_{\hat d}}$ — the estimate after the update, its typical error
$\textcolor{#1f77b4}{d^-}$, $\textcolor{#1f77b4}{\sigma_{d^-}}$ — the prediction, its typical error: the filter computes both in predict
$\textcolor{#d62728}{z}$ — the ray's reading at this scan; $\textcolor{#d62728}{\sigma_z}$ — its typical error, from the datasheet or measured standing: we set it once
$K$ — the Kalman gain, the scan's best weight
$\sigma^2$ — a variance; ±σ holds 68 %
start: 51 ± 2 cm (example); the ray 49.7 ± 1.01 cm, measured
mean1, var1 — $d^-$, $\sigma_{d^-}^2$; mean2, var2 — $z$, $\sigma_z^2$; mean, var — $\hat d$, $\sigma_{\hat d}^2$

Legend
green lines — the filter's estimate from three starts: solid — correct, the true 2 m, $\sigma_0 = \sigma_z$; dashed — "I do not know", 0 m, $\sigma_0$ = 10 m; dotted — wrong and sure, 0.5 m short, $\sigma_0$ = 1 mm; hollow dots — the starts; the bold dot — within 2 cm
black — the truth, Gazebo's ground truth; red dots — the scans $z$ of the forward ray
$K_1$ — the Kalman gain of the first scan
$\hat d_0$ — the first guess of the distance to the wall; $\textcolor{#1f77b4}{\sigma_0}$ — its typical error: how far it may be from the truth
$\textcolor{#d62728}{\sigma_z}$ — the lidar's typical error, 10.1 mm on our drive

Legend
Q — $\sigma_u^2$, one odometry step's doubt
R — $\sigma_z^2$, the lidar's doubt
x — the state: $d^-$, then $\hat d$
P — its variance: $\sigma_{d^-}^2$, then $\sigma_{\hat d}^2$
y — the scan minus the prediction, $z - d^-$
$K$ — the Kalman gain; I_KH — $1 - K$
$F$ — the state's own change: 1 here
$B\,u$ — the step $-v\,\Delta t$: speed × time
$H$ — picks what the sensor reads: 1
dot — the matrix product, rows × columns; for numbers, ×
.T — turned over; _alpha_sq — 1

Legend
black — the truth, zero error; red dots — each scan's error; green — the estimate's error, its band $\pm 2\sigma_{\hat d}$
$\textcolor{#d62728}{\sigma_z}$ — the lidar's typical error; filterpy's R = $\sigma_z^2$
$\sigma_u$ — one step's distance error, m: the speed error held for $\Delta t$; filterpy's Q = $\sigma_u^2$
$\sigma_v$ — the wheels' speed error, m/s, from the wheels' driver
$\Delta t$ — the time between two scans, s
we set $\sigma_z$ and $\sigma_u$ once; the slider multiplies $\sigma_u^2$, $\sigma_z$ stays as measured
$K$ — the scan's weight; $\sigma_{\hat d}$ — the estimate's typical error

Legend
$P$ — the covariance matrix: the doubt of several numbers; 3 × 3 for the pose $(x, y, \theta)$, $\theta$ — the yaw; here only $x$, $y$
$\sigma_x^2$, $\sigma_y^2$ — the variance of each, cm²: on the diagonal, the same in both clouds
$\mathrm{cov}(x, y)$ — the covariance: multiply the two errors of each pair, average over many pairs, cm²; off the diagonal; 0 when the errors are independent
$x$, $y$ — the position errors, cm
green dots — many position errors; black cross — zero error, the true position
A, B — both errors the same way: $x \cdot y > 0$; C, D — opposite ways: $x \cdot y < 0$

Legend
grey dots — 1000 simulated drives of 2 m with our wheels' noise
blue dots — 20 Gazebo runs: where the wheel odometry says each ended
green — their 2σ ellipsoid; dashed — its shadow, the 2σ ellipse of $x$ and $y$
$x$ — the error along the drive, $y$ — sideways, mm; $\theta$ — the yaw error, °
$P$ — the covariance matrix of $(x, y, \theta)$
$\sigma_x^2$, $\sigma_y^2$, $\sigma_\theta^2$ — the variance of each error: the diagonal
$\mathrm{cov}(y, \theta)$ — the covariance of the $y$ and yaw errors, > 0; zeros — $x$ depends on neither (≈ 0 on these drives)

Legend
grey lines — copies of the robot driving 2 m, each with its own yaw error; blue dots — where they end; grey rings — before the drive
$y$ — the sideways error, m; $\theta$ — the yaw error, rad: ±0.1 rad ≈ ±6° (example); the minus — after the step
$F$ — the step as a table: a row per new value, a column per old one. Not a covariance: the motion
$P$, $\textcolor{#1f77b4}{P^-}$ — the covariance of $(y, \theta)$ before and after the step
$F^\top$ — $F$ turned over: rows become columns
$FPF^\top$ — the doubt through the step: an error times $a$ has the variance $a^2\sigma^2$, so $F$ stands on both sides
2 m × $\theta$: for small angles, $\sin\theta \approx \theta$

Legend
black — Gazebo's ground truth; orange dashed — the Kalman filter stepping along $\theta_0$ (filterpy); dots — now
position error — the distance from the filter's $(x, y)$ to the truth, m; grey band — the turn
$x^-$, $y^-$ — the predicted position; $\hat x$, $\hat y$ — the estimates, m
$v\,\Delta t$ — one step: speed × time
$\hat\theta$ — the yaw now; $\theta_0$ — the yaw at the start, a fixed number
$F$, $B$ — the Kalman filter's fixed tables; $u$ — the step's input, $v\,\Delta t$

Legend
green — what goes in; blue — what comes out: 200 000 random inputs pushed through
black — the line and the curve: what one step does to the number
grey dashed — the input's centre carried through; blue dot — the mean of what comes out
$v$ — the speed; $\Delta t$ — the step's time; $v\,\Delta t$ — the step, m
$\theta$, $\hat\theta$ — the yaw, its estimate; $\Delta x$ — the step's part along $x$
$\sigma$ — the typical error, the ± on the picture; $\sigma^2$ — the doubt, its variance
a line with slope 2: $\sigma$ × 2, $\sigma^2$ × 4

Legend
black — $\cos\theta$, the curve; blue dashed — its tangent lines at 60° and 0°; green — the yaw's Gaussian, ±10°; left — what comes out
$\Delta x$ — the step along $x$; $v\,\Delta t$ — the step: speed × time
$\theta$ — the yaw; $\hat\theta$ — its estimate; $\theta_0$ — the yaw at the start
tangent line — the straight line touching the curve at $\hat\theta$, not $\tan\theta$; its slope — the derivative, $(\cos\theta)' = -\sin\theta$
sy, cy, cp — sin, cos of yaw, cos of pitch (1 on the floor); x_vel, delta_sec — $v$, $\Delta t$; y_vel, z_vel — 0
x_coeff, dFx_dY — the slopes of cos and of $x$ in yaw

Legend
black — the truth; orange dashed — the Kalman filter; green — the EKF
position error — the distance from the filter's $(x, y)$ to the truth, m
$d$ — the distance to the wall; $x$, $\theta$ — the position, the yaw
a hat — the estimate; a minus — the prediction, «before the measurement»
$v\,\Delta t$ — one step
$P$, $P^-$ — the doubt (covariance) of the estimate, of the prediction
$F$ — the step's slopes; $F^\top$ — $F$ turned over; $Q$ — what a step adds, set by us; $K$ — the Kalman gain

Legend
blue — what goes in; green — what comes out
vx, vy, vyaw — the speed forward, sideways and of turning
$(x, y, \theta)$ — the pose: the position and the yaw
$P$ — the covariance: the doubt of every number of the state
TF odom → base_link — where the robot's body is from the place the odometry started
publish_tf — whether ekf_node sends that TF

Legend
light blue — the gyroscope's yaw; purple — with $\hat b$ subtracted; purple strip — the first 5 s, where $\hat b$ is measured; black — the truth
$\Delta\theta$ — the yaw error; $t$ — time
$b$ — the bias: the same small error in every reading; $\textcolor{#7b3fa0}{\hat b}$ — the bias, measured
$\textcolor{#6baed6}{\omega}$ — the gyroscope's turn rate, rad/s; $\omega - \hat b$ — without the bias
checkState — does the robot stand? ax…wz — the IMU's readings
wz_bias_ — $\hat b$, a running mean; bias_alpha_ — a new reading's weight, 0.01
wz_unb — $\omega - \hat b$