Lecture 6 — State Estimation and Fusion (I)

Legend
tilt — how far the robot leans: the angle between its up axis and the vertical
roll — the robot's tilt to the side, °
black line — the true roll, from Gazebo
blue line — the roll from the gyroscope
red line — the roll from the accelerometer
grey vertical line — the moment the pictures show
red arrow in RViz — what the accelerometer feels; RViz holds the robot model level
dark boxes — the roll at this moment: the truth in Gazebo, the accelerometer's in RViz

Legend
$\textcolor{#1f77b4}{\phi_{gyro}}$, $\textcolor{#d62728}{\phi_{acc}}$ — the roll from the gyroscope and from the accelerometer, rad
$\omega$ — the turn rate from the gyroscope, rad/s
$\Delta t$ — one IMU step: 0.01 s at 100 Hz
$b$ — the gyroscope's bias: the same small error in every reading
$t$ — the time since the start, s
$a_y, a_z$ — the accelerometer along the robot's own axes: sideways and up, m/s²
$\mathrm{atan2}$ — the angle of a vector from its two parts, with the right sign
blue steps — the gyroscope's sum; dashed — the truth
red arrows — gravity's push $a$ and its two parts

Legend
$\textcolor{#1f77b4}{\phi^-}$ — the prediction: the old estimate turned by the gyroscope; the minus — before the measurement corrects it, the usual mark of a prediction
$\hat\phi_{k-1}$ — the old estimate; the hat — an estimate, not the truth; $k$ — the step number
$\omega\,\Delta t$ — the gyroscope's turn in one step: rate $\omega$ × one IMU step $\Delta t$, 0.01 s
$\textcolor{#d62728}{\phi_{acc}}$ — the accelerometer's roll: the angle of gravity's push from its two parts, $\mathrm{atan2}(a_y, a_z)$
$a_y, a_z$ — the accelerometer along the robot's own axes: sideways and up, m/s²
$\textcolor{#2ca02c}{\hat\phi_k}$ — the new estimate
the gap $\textcolor{#d62728}{\phi_{acc}} - \textcolor{#1f77b4}{\phi^-}$ — how far the accelerometer is from the prediction
$\alpha$ — the weight of the prediction, between 0 and 1; $1-\alpha$ — the accelerometer's

Legend
blue ring — $\textcolor{#1f77b4}{\phi^-}$, the gyroscope's prediction: the old estimate plus one gyroscope step, not its whole sum
red dot — $\textcolor{#d62728}{\phi_{acc}}$, the accelerometer's roll: the angle of gravity's push from its two parts, $\mathrm{atan2}(a_y, a_z)$
$a_y, a_z$ — the accelerometer along the robot's own axes: sideways and up, m/s²
green dot — $\textcolor{#2ca02c}{\hat\phi_k}$, the new estimate; $k$ — the step number
dotted line — the truth, 10° in all three cases
the gap $\textcolor{#d62728}{\phi_{acc}} - \textcolor{#1f77b4}{\phi^-}$ — how far the accelerometer is from the prediction
0.02 — the accelerometer's share, $1-\alpha$, with $\alpha = 0.98$

Legend
green curve — $\textcolor{#2ca02c}{\Delta\phi_n}$ over time; dots — the start and after 10, 50, 100 steps
dotted line — the moment $t = \tau$
$\textcolor{#2ca02c}{\Delta\phi_n}$ — the roll error after $n$ steps: the estimate minus the true roll, °; $\Delta\phi_0$ — at the start, 1° here
$\textcolor{#2ca02c}{\hat\phi_k}$ — the new estimate; $k$ — the step number
$\textcolor{#1f77b4}{\phi^-}$ — the gyroscope's prediction
$\textcolor{#d62728}{\phi_{acc}}$ — the accelerometer's roll
$\alpha$ — the prediction's weight, 0.98 here; $1-\alpha$ — the accelerometer's share
$n$ — the number of steps
$\tau$ — the time constant, s
$\Delta t$ — one IMU step: 0.01 s at 100 Hz

Legend
axis — the line the turn leaves in place, as three numbers along $x, y, z$; for the roll $(1, 0, 0)$ — $x$, forward
angle — how far it turns; here the roll $\phi$ = 9.7°, from the dashed vertical to the robot's $z$
$x, y, z, w$ — the four numbers of a quaternion, as ROS names them; $x, y, z$ go with the axes $x, y, z$
$\sin$, $\cos$ of half the angle — a quaternion keeps half of it: 4.85°
red ⊗, green, blue — the robot's axes $x$ (into the screen), $y$, $z$, as RViz colours them, and their numbers in the quaternion

Legend
green line — $\textcolor{#2ca02c}{\hat\phi_k}$, the node's estimate; $k$ — the step number
light red line — $\textcolor{#d62728}{\phi_{acc}}$, the accelerometer's roll
black line — the truth
$\textcolor{#1f77b4}{\phi^-}$ — the gyroscope's prediction
$\alpha$ — the prediction's weight
gain_acc — imu_tools' name for the accelerometer's share, $1-\alpha$
$\tau$ — the time constant, s; $\Delta t$ — one IMU step, 0.01 s
RMS error — the root mean square of the node's roll minus the true roll, over the run: its typical error

Legend
dashed line — the true roll, from Gazebo
green line — the node's roll
faint red — the accelerometer; faint blue — the gyroscope
grey vertical line — the moment Gazebo shows
$b$ — the gyroscope's bias: the same small error in every reading
$\Delta t$ — one IMU step, 0.01 s
$\tau$ — the time constant: about 1 s at gain_acc 0.01
yaw — the turn around the vertical

Legend
$\mathcal N(\hat d, \sigma_{\hat d}^2)$ — a normal distribution, a Gaussian: centre $\hat d$, variance $\sigma_{\hat d}^2$
$\hat d$ — the best guess of the distance, m; the hat — an estimate
$\sigma_{\hat d}$ — its typical error: the Gaussian's width; $\sigma^2$ — a variance
$\textcolor{#d62728}{z}$ — one scan of the lidar's forward ray, m
$\textcolor{#d62728}{\sigma_z}$ — how far one scan scatters: 10.1 mm, measured
red bars — the scans, 2 mm bins, the latest bright; red curve — where one scan lands
green number — $\hat d$, the mean so far; dashed — the truth
probability — the area under the curve; ±σ holds 68 %

Legend
$\sigma_z$ — the error of one reading (one scan)
$n$ — the number of readings averaged
$\bar z$ — their average; a bar means an average
$\sigma_{\bar z}$ — the error of their average: the root of the mean square
16 = 2⁴ sign combinations of four readings; 1-4-6-4-1 — how many of them give each sum
red — one reading; green — the average of 4 and of 16: bars — 40,000 tries, curves — the Gaussian of the formula, arrows — ±σ

Legend
$n$ — the number of the scan
$\textcolor{#d62728}{z_n}$ — the $n$-th scan of the lidar's forward ray, m
$\textcolor{#2ca02c}{\hat d_n}$ — the average of the first $n$ scans; $\hat d_{n-1}$ — before the new one
$\frac{1}{n}$ — the share of the new scan
$z_n - \hat d_{n-1}$ — how much the new scan disagrees
same as $(z_1 + \dots + z_n)/n$: multiply both sides by $n$
z — all scans in a pandas column (Series), one per row
expanding() — windows that grow by one scan: $z_1$; $z_1, z_2$; $z_1, z_2, z_3$; …
.mean() — each window's average: d_hat = $\hat d_1, \hat d_2, \dots$
red dots — the scans; green line — their average; black line — the truth, 2.000 m
light band — ±σ_z/√n: the average's expected error; σ_z — one scan's error, 10.1 mm

Legend
lag — how far the average is behind the truth, cm
$n$ — scans in the window, 20
$v$ — speed toward the wall, m/s
$\Delta t$ — time between scans, 0.1 s
$d$ — the true distance to the wall, $z$ — the lidar's forward ray, m
$\frac{n-1}{2}$ — the window's middle lags this many scans: the mean of 0, 1, …, $n-1$
z — all scans in a pandas column (Series), one per row
rolling(20) — a window of the last 20 scans that slides by one: $z_1 \dots z_{20}$; $z_2 \dots z_{21}$; …
.mean() — each window's average: d_avg, the orange line
black — the truth · red — the ray · orange — the mean of the last 20 scans
grey vertical line — the moment the scene shows; dots — the truth and the mean at it

Legend
$d$ — a cell: a distance to the wall, m
$z$ — the scan: what the lidar's forward ray reads, m
$p(\cdot)$ — a probability, 0 to 1; the bar $\mid$ reads «given»
$\textcolor{#1f77b4}{p(d)}$ — the belief before the scan (prior): blue bars
$\textcolor{#d62728}{p(z \mid d)}$ — the likelihood: how likely the ray reads $z$ if the wall is at $d$: red bars
purple bars — the products, cell by cell
$p(z)$ — their sum; divided by it, the bars add up to 1
$\textcolor{#2ca02c}{p(d \mid z)}$ — the belief after the scan (posterior): green bars

Legend
$d$ — a cell after the motion: a distance to the wall, m
$d'$ — a cell before it; the sum runs over all of them
$u$ — the motion: the wheels' step $v\,\Delta t$, here 1.8 cm
$\textcolor{#2ca02c}{bel(d')}$ — the belief after the last scan: green bars
$p(d \mid u, d')$ — the chance that the step $u$ takes the robot from $d'$ to $d$: the wheels err
$\textcolor{#1f77b4}{\overline{bel}(d)}$ — the belief before the next scan: blue bars; the bar on top — before the scan
outlined bars — the green bars, moved by $u$: each blue bar collects from those around it
the belief — one number per cell; the robot keeps nothing else

Legend
$d$ — a cell: a distance to the wall, m
$z$ — the scan: what the lidar's forward ray reads, m
$\textcolor{#1f77b4}{\overline{bel}(d)}$ — the belief before the scan, from predict: blue bars
$\textcolor{#d62728}{p(z \mid d)}$ — the likelihood: how likely the scan $z$ is if the wall is at $d$; a Gaussian around $z$, as wide as the lidar's error $\sigma_z$: red curve
$\textcolor{#2ca02c}{bel(d)}$ — the belief after the scan: green bars; outlined — the blue bars, to compare
$\eta$ — 1 / the sum of the products, so the bars add up to 1: the $1/p(z)$ of Bayes' rule
dashed line — the truth

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$