Exercises — Unscented Kalman Filter (UKF) — sigma points, better for nonlinear

WHAT: count entries of $x$. There are $4$ numbers, so $n=4$. WHY: the parent gives one center point plus one point on each side of every principal axis, i.e. $2n+1$. $2n+1=2(4)+1===9==$ Answer: 9 sigma points.

• (a) $W_0^{(m)}$ = center mean weight.
• (b) $\lambda$ = scaling parameter.
• (c) $W_0^{(c)}$ = center covariance weight (same as $W_0^{(m)}$ plus the Gaussian-tuning term $(1-{\alpha }^2+\beta )$). WHY it matters: (a) and (c) differ only at the center. That is the whole trick that lets us tune covariance without shifting the mean.

Step 1 — $\lambda$ (WHY: it fixes how far/weighted points are): $\lambda ={\alpha }^2(n+\kappa )-n=1\cdot (2+1)-2===1==$ So $n+\lambda =2+1=3$. Step 2 — center weights: $W_0^{(m)}=\frac{\lambda }{n+\lambda }=\frac{1}{3}$ $W_0^{(c)}=\frac{1}{3}+(1-{\alpha }^2+\beta )=\frac{1}{3}+(1-1+2)=\frac{1}{3}+2=\frac{7}{3}$ Step 3 — outer weights (there are $2n=4$ of them): $W_i^{(m)}=W_i^{(c)}=\frac{1}{2(n+\lambda )}=\frac{1}{6}$ Sanity check (mean weights must sum to 1): $\frac{1}{3}+4\cdot \frac{1}{6}=\frac{1}{3}+\frac{2}{3}=1$. ✔

WHAT: we need the spread distance $\sqrt{(n+\lambda )P}$. WHY the square root: covariance measures spread² (variance); to turn it into an actual distance along the axis we take its square root — same reason standard deviation is $\sqrt{\text{variance}}$. $\sqrt{(n+\lambda )P}=\sqrt{3\cdot 4}=\sqrt{12}=2\sqrt{3}\approx 3.464$ ${\mathcal{X}}_0=5,{\mathcal{X}}_1=5+2\sqrt{3}\approx 8.464,{\mathcal{X}}_2=5-2\sqrt{3}\approx 1.536$ Look at Figure s01: the red center point sits at $5$, and two black points flank it symmetrically.

(a) UT. Sigma points: ${\mathcal{X}}_0=0,{\mathcal{X}}_{1,2}=\pm \sqrt{3}\sigma$. Weights $W_0=\frac{2}{3},W_{1,2}=\frac{1}{6}$. Transform each: ${\mathcal{Y}}_0=0^2=0,{\mathcal{Y}}_{1,2}=(\sqrt{3}\sigma {)}^2=3{\sigma }^2$. ${\bar{y}}_{\text{UT}}=\frac{2}{3}(0)+\frac{1}{6}(3{\sigma }^2)+\frac{1}{6}(3{\sigma }^2)={\sigma }^2$ (b) EKF. Linearize: $F={\frac{d}{dx}{x}^2|}_0=2x{|}_0=0$. So ${\bar{y}}_{\text{EKF}}=f(0)+F\cdot 0=0$. (c) The true mean is ${\sigma }^2$. The UT is exact; the EKF is completely wrong (predicts 0). WHY: at $x=0$ the slope of $x^2$ is flat, so the EKF's linear model "sees" no change — it is blind to curvature. The UT samples $f$ at points away from the mean, so it feels the upward bend and recovers the ${\sigma }^2$ lift. Look at Figure s02: black parabola, red sigma points landing on the curve well above the flat tangent (dashed).

(a) $\lambda ={\alpha }^2(n+\kappa )-n=(10^{-6})(1)-1=10^{-6}-1$. Then $n+\lambda =1+(10^{-6}-1)=10^{-6}$. (b) Distance $=\sqrt{(n+\lambda )P}=\sqrt{10^{-6}}\sigma =10^{-3}\sigma$. Points at $\pm 10^{-3}\sigma$ — hugging the mean. (c) The covariance weight $W_i^{(c)}=\frac{1}{2(n+\lambda )}=\frac{1}{2\cdot 10^{-6}}=5\times 10^5$ is huge, exactly cancelling the tiny squared distances so $P_y$ reconstructs correctly. Spread ↓, weight ↑ — they trade off by design.

Step 1 — sigma points. Spread $=\sqrt{3\cdot 4}=\sqrt{12}=2\sqrt{3}$. ${\mathcal{X}}_0=1,{\mathcal{X}}_1=1+2\sqrt{3},{\mathcal{X}}_2=1-2\sqrt{3}$ Numerically: ${\mathcal{X}}_1\approx 4.4641$, ${\mathcal{X}}_2\approx -2.4641$. Step 2 — push through $f$.

• ${\mathcal{X}}_0^{-}=f(1)=1+0.5=1.5$
• ${\mathcal{X}}_1^{-}=f(4.4641)=4.4641+0.5(4.4641{)}^2=4.4641+9.9641=14.4282$
• ${\mathcal{X}}_2^{-}=f(-2.4641)=-2.4641+0.5(-2.4641{)}^2=-2.4641+3.0359=0.5718$ Step 3 — predicted mean (weights $W_0=\frac{2}{3}$, $W_{1,2}=\frac{1}{6}$): ${\hat{x}}_k^{-}=\frac{2}{3}(1.5)+\frac{1}{6}(14.4282)+\frac{1}{6}(0.5718)=1.0+2.5=3.5$ Step 4 — predicted covariance (add $Q$! this is the classic trap): $P_k^{-}={\sum }_i{W}_i^{(c)}({\mathcal{X}}_i^{-}-{\hat{x}}_k^{-}{)}^2+Q$ Deviations: $1.5-3.5=-2$; $14.4282-3.5=10.9282$; $0.5718-3.5=-2.9282$. $=\frac{2}{3}(4)+\frac{1}{6}(119.4256)+\frac{1}{6}(8.5744)+1$ $=2.6667+19.9043+1.4291+1=25.0000$ Answer: ${\hat{x}}_k^{-}=3.5$, $P_k^{-}=25$. (Notice the covariance ballooned from 4 to 25 — the curvature of $f$ stretched the uncertainty, and the UT captured it.)

Step 1 — new sigma points. Spread $=\sqrt{3\cdot 25}=\sqrt{75}=5\sqrt{3}\approx 8.6603$. ${\mathcal{X}}_0^{-}=3.5,{\mathcal{X}}_1^{-}=3.5+5\sqrt{3}\approx 12.1603,{\mathcal{X}}_2^{-}=3.5-5\sqrt{3}\approx -5.1603$ Step 2 — push through $h(x)=2x$: ${\mathcal{Z}}_0=7,{\mathcal{Z}}_1=24.3205,{\mathcal{Z}}_2=-10.3205$ Step 3 — predicted measurement mean: ${\hat{z}}_k=\frac{2}{3}(7)+\frac{1}{6}(24.3205)+\frac{1}{6}(-10.3205)=4.6667+2.3333=7.0$ (Makes sense: $h$ is linear, so ${\hat{z}}_k=2{\hat{x}}_k^{-}=7$.) Step 4 — measurement covariance (add $R$!): Deviations ${\mathcal{Z}}_i-{\hat{z}}_k$: $0$, $17.3205$, $-17.3205$. $P_{zz}=\frac{2}{3}(0)+\frac{1}{6}(300)+\frac{1}{6}(300)+R=100+1=101$ Step 5 — cross-covariance (no $R$ here): State deviations ${\mathcal{X}}_i^{-}-{\hat{x}}_k^{-}$: $0$, $8.6603$, $-8.6603$. $P_{xz}=\frac{1}{6}(8.6603)(17.3205)+\frac{1}{6}(-8.6603)(-17.3205)=25+25=50$ Step 6 — Kalman gain: $K=P_{xz}{P}_{zz}^{-1}=\frac{50}{101}\approx 0.49505$ Step 7 — corrected state: ${\hat{x}}_k={\hat{x}}_k^{-}+K(z_k-{\hat{z}}_k)=3.5+0.49505(6-7)=3.5-0.49505\approx 3.00495$ Step 8 — corrected covariance: $P_k=P_k^{-}-K{P}_{zz}K=25-0.49505^2(101)=25-24.7525\approx 0.24752$ Answers: $K\approx 0.4950$, ${\hat{x}}_k\approx 3.005$, $P_k\approx 0.2475$. WHY the covariance shrank so hard: the measurement was very informative ($h=2x$ directly scales the state), so the update pulled the estimate toward $z/2=3$ and slashed uncertainty from 25 to ~0.25.

EKF: at the origin $\frac{x^{\top }}{\Vert x\Vert }=\frac{0}{0}$ — undefined. The linearization simply does not exist, so the EKF has no gain to compute. UKF: $P_{xz}$ is a finite weighted sum of products $({\mathcal{X}}_i-0)\cdot (\Vert {\mathcal{X}}_i\Vert -\bar{z})$. Every term is a plain multiplication of numbers we can evaluate — no derivative, no division by zero. The sigma points sit away from the origin, so $\Vert {\mathcal{X}}_i\Vert \ne 0$ and the sum is well-defined. The lesson: the UKF never asks "what is the slope here?" It asks "how does the output co-vary with the input across a spread of samples?" — a question with an answer even where the slope has none.