Exercises

$\mathbf{F} = \begin{pmatrix} 1 & T_s \\ 0 & 1 \end{pmatrix}$, $\mathbf{G} = (T_s^2/2,\; T_s)^T$, $\mathbf{Q} = q$.

$\mathbf{H} = (1, 0)$, $\mathbf{R} = r$.

$P[1|0] = 1\cdot 0 + 1 = 1$.

$K[1] = 1/(1+1) = 1/2$, so $P[1|1] = (1-K[1])\cdot P[1|0] = 1/2$.

$\mathbf{P}[n|n] = (\mathbf{I}-\mathbf{K}\mathbf{H})\mathbf{P}[n|n-1](\mathbf{I}-\mathbf{K}\mathbf{H})^T + \mathbf{K}\mathbf{R}\mathbf{K}^T$.

$\partial/\partial \mathbf{K}\, \mathrm{tr}(\mathbf{P}[n|n]) = -2(\mathbf{I}-\mathbf{K}\mathbf{H})\mathbf{P}[n|n-1]\mathbf{H}^T + 2\mathbf{K}\mathbf{R} = 0$, giving $\mathbf{K} = \mathbf{P}[n|n-1]\mathbf{H}^T(\mathbf{H}\mathbf{P}[n|n-1]\mathbf{H}^T + \mathbf{R})^{-1}$.

$\mathbb{E}[\boldsymbol{\nu}[n]] = \mathbf{H}\mathbb{E}[\mathbf{x}[n] - \hat{\mathbf{x}}[n|n-1]] + \mathbb{E}[\mathbf{v}[n]] = 0$.

$\mathrm{Cov}(\boldsymbol{\nu}[n]) = \mathbf{H}\mathbf{P}[n|n-1]\mathbf{H}^T + \mathbf{R}$ since the prediction error and observation noise are independent.

$(P+R)P = F^2 P (P+R) - F^2 P^2 + Q(P+R)$, which reduces to $P^2 + (R - F^2 R + Q - 1) P - QR = 0$.

With $F=0.9$, $R=1$, $Q=0.1$: $P^2 + (1-0.81+0.1-1)P - 0.1 = 0$, i.e., $P^2 - 0.71 P - 0.1 = 0$. Positive root: $\bar{P} = (0.71 + \sqrt{0.71^2 + 0.4})/2 \approx 0.834$.

$\mathbf{P}[n|n] = \mathbf{P}[n|n-1] - \mathbf{P}[n|n-1]\mathbf{H}^T (\mathbf{H}\mathbf{P}[n|n-1]\mathbf{H}^T+\mathbf{R})^{-1}\mathbf{H}\mathbf{P}[n|n-1]$.

With $\mathbf{A}=\mathbf{P}[n|n-1]^{-1}$, $\mathbf{U}=\mathbf{H}^T$, $\mathbf{V}=\mathbf{H}$, $\mathbf{C}=\mathbf{R}^{-1}$, Woodbury gives $\mathbf{P}[n|n]^{-1} = \mathbf{P}[n|n-1]^{-1} + \mathbf{H}^T\mathbf{R}^{-1}\mathbf{H}$.

Substitute the update into the predict step to get $\hat{\mathbf{x}}[n|n] = \mathbf{A}\hat{\mathbf{x}}[n-1|n-1] + \bar{\mathbf{K}}\mathbf{y}[n]$ where $\mathbf{A} = (\mathbf{I}-\bar{\mathbf{K}}\mathbf{H})\mathbf{F}$.

$\hat{\mathbf{X}}(z) = (z\mathbf{I}-\mathbf{A})^{-1} z \bar{\mathbf{K}}\mathbf{Y}(z)$. This is the state-space realization of the causal Wiener filter for the signal model.

$\bar{P}^2 = q(\bar{P} + r)$, i.e., $\bar{P}^2 - q\bar{P} - qr = 0$.

$\bar{P} = (q + \sqrt{q^2 + 4qr})/2$; steady-state gain $\bar{K} = \bar{P}/(\bar{P}+r)$, filtering MSE $\bar{P}(1-\bar{K}) = \bar{P} r/(\bar{P}+r)$.

For a jointly Gaussian pair, $\mathbb{E}[\mathbf{x}|\mathbf{y}]$ is an affine function of $\mathbf{y}$ with coefficients given by the Wiener-Hopf normal equations. The Kalman gain is exactly that coefficient at step $n$.

For jointly Gaussian random variables the conditional mean is linear, so the best affine estimator is the conditional mean, which is the MMSE estimator.

$f(\mathbf{x}[n]) \approx f(\hat{\mathbf{x}}[n|n]) + \mathbf{F}_n(\mathbf{x}[n] - \hat{\mathbf{x}}[n|n])$.

Replace $\mathbf{F}$ by $\mathbf{F}_n$ in the covariance propagation; propagate the mean through the nonlinearity: $\hat{\mathbf{x}}[n+1|n] = f(\hat{\mathbf{x}}[n|n])$, $\mathbf{P}[n+1|n] = \mathbf{F}_n \mathbf{P}[n|n] \mathbf{F}_n^T + \mathbf{Q}$.

The EKF linearizes around the current mean; it is only accurate to first order in $\mathbf{x}-\hat{\mathbf{x}}$.

The UKF deterministically samples $2n+1$ sigma points that match the mean and covariance, propagates each through the true nonlinearity, and recomputes moments. This captures mean and covariance to (at least) second order of the Taylor expansion.

$\hat{\mathbf{x}}[n+k|n] = \mathbf{F}^k \hat{\mathbf{x}}[n|n]$.

$\mathbf{P}[n+k|n] = \mathbf{F}^k \mathbf{P}[n|n] (\mathbf{F}^k)^T + \sum_{j=0}^{k-1} \mathbf{F}^j \mathbf{G}\mathbf{Q}\mathbf{G}^T (\mathbf{F}^j)^T$.

Initialize with $\mathbf{P}_0 = \mathbf{0}$. The Riccati recursion produces a monotonically non-decreasing sequence $\mathbf{P}_n \preceq \mathbf{P}_{n+1}$ bounded above under detectability, hence convergent. The limit satisfies the DARE.

Suppose two PSD solutions $\mathbf{P}_1, \mathbf{P}_2$ existed. Then $\mathbf{P}_1 - \mathbf{P}_2$ satisfies a Lyapunov equation with a stable closed-loop matrix under stabilizability, forcing $\mathbf{P}_1 = \mathbf{P}_2$. See Anderson-Moore, Ch. 4.

$\mathbf{x} = (p_x, p_y, v_x, v_y)^T$, $\mathbf{F}$ is block-upper triangular with $T_s$ coupling. The EKF linearizes the arctan observation via $\mathbf{H}_n = \dfrac{1}{p_x^2+p_y^2}(-p_y,\, p_x,\, 0,\, 0)$.

Any target on a ray from the origin at any range produces the same bearing sequence. The observer must manoeuvre to break this symmetry and make the system locally observable.

Fix: use a square-root or Joseph-form update to maintain $\mathbf{P} \succeq \mathbf{0}$ and symmetric at every step.

Underestimated $\mathbf{Q}$ makes the filter overconfident; it ignores new measurements. Fix: inflate $\mathbf{Q}$, or add adaptive noise estimation (IMM / Mehra's innovations method).