Kalman Filter in 3 Ways

Slide: Discover the Kalman Filter through the Geometric perspective of orthogonal projection, the Probabilistic perspective of Bayesian filtering, and the Optimization perspective of weighted least squares. Inspired by Ling Shi's 2024-25 Spring lecture "Networked Sensing, Estimation and Control".

Introduction

System Model and Assumptions

Consider a discrete-time linear Gaussian system with initial condition $x_0$ and $P_0$:

$$\begin{array}{rlr}{x}_{k+1}& =A_k{x}_k+B_k{u}_k+{\omega }_k,& {\omega }_k\sim \mathcal{N}(0,Q_k)\\ y_k& =C_k{x}_k+{\nu }_k,& {\nu }_k\sim \mathcal{N}(0,R_k)\end{array}$$

Assumptions:

• $(A_k,B_k)$ is controllable and $(A_k,C_k)$ is observable
• $Q_k⪰0,R_k⪰0,P_0⪰0$
• ${\omega }_k$, ${\nu }_k$ and $x_0$ are mutually uncorrelated
• The future state of the system is conditionally independent of the past states given the current state

Goal: Find ${\hat{x}}_{k|k}=\mathbb{E}[x_k|y_{1:k}]$ (MMSE estimator)

Geometric Perspective: Orthogonal Projection

Hilbert Space of Random Variables

Key Idea:

• View random variables as vectors in Hilbert space
• Inner product: $⟨\xi ,\eta ⟩=\mathbb{E}[\xi \eta ]$
• Orthogonality: $\xi \perp \eta \iff \mathbb{E}[\xi \eta ]=0$
• Optimal estimate is orthogonal projection onto observation space

Geometric Interpretation:

Courtesy: https://math.stackexchange.com/users/117818/qqo

Time Update

State Prediction:

$$\begin{array}{rl}{\hat{x}}_{k|k-1}& =\mathbb{E}[x_k\mid y_{1:k-1}]\\ & =\mathbb{E}[A_{k-1}{x}_{k-1}+B_{k-1}{u}_{k-1}+w_{k-1}\mid y_{1:k-1}]\\ & =A_{k-1}{\hat{x}}_{k-1|k-1}+B_{k-1}{u}_{k-1}\text{(since }{w}_{k-1}\perp y_{1:k-1}\text{)}\end{array}$$

Covariance Prediction:

$$\begin{array}{rl}{P}_{k|k-1}& =\text{cov}(x_k-{\hat{x}}_{k|k-1})\\ & =\text{cov}[A_{k-1}(x_{k-1}-{\hat{x}}_{k-1|k-1})+w_{k-1}]\\ & =A_{k-1}\cdot \text{cov}(x_{k-1}-{\hat{x}}_{k-1|k-1})\cdot A_{k-1}^{\mathrm{\top }}+2A_{k-1}\cdot \text{cov}(x_k-{\hat{x}}_{k|k-1},{\omega }_{k-1})+\text{cov}(w_{k-1})\\ & =A_{k-1}{P}_{k-1|k-1}{A}_{k-1}^{\mathrm{\top }}+Q_{k-1}\end{array}$$

Innovation Process

Definition:

$$\begin{array}{rl}{e}_k& =y_k-{\hat{y}}_{k|k-1}\\ & =y_k-{\text{proj}}_{{\mathcal{Y}}_{k-1}}(y_k)\\ & =y_k-{\text{proj}}_{{\mathcal{Y}}_{k-1}}(C_k{x}_k+{\nu }_k)\\ & =y_k-C_k\cdot {\text{proj}}_{{\mathcal{Y}}_{k-1}}(x_k)-{\text{proj}}_{{\mathcal{Y}}_{k-1}}({\nu }_k)\\ & =y_k-C_k{\hat{x}}_{k|k-1}\end{array}$$

Properties:

• Zero Mean: $\mathbb{E}[e_k]=0$
• White Sequence: $\mathbb{E}[e_k{e}_j^{\mathrm{\top }}]=0$ for $k\ne j$
• Orthogonality Principle: $\mathbb{E}[e_k{y}_j^{\mathrm{\top }}]=0$ for $j<k$

Measurement Update

State Update:

$$\begin{array}{rl}{\hat{x}}_{k|k}& ={\text{proj}}_{{\mathcal{Y}}_k}(x_k)\\ & ={\hat{x}}_{k|k-1}+K_k{e}_k\\ & ={\hat{x}}_{k|k-1}+K_k(y_k-C_k{\hat{x}}_{k|k-1})\end{array}$$

Covariance Update:

$$\begin{array}{rl}{P}_{k|k}& =\text{cov}(x_k-{\hat{x}}_{k|k})\\ & =\text{cov}(x_k-{\hat{x}}_{k|k-1}-K_k{e}_k)\\ & =\text{cov}(x_k-{\hat{x}}_{k|k-1})-2K_k\text{cov}(x_k-{\hat{x}}_{k|k-1},e_k)+K_k\text{cov}(e_k)K_k^{\mathrm{\top }}\\ & =\text{cov}(x_k-{\hat{x}}_{k|k-1})-2K_k\text{cov}(x_k-{\hat{x}}_{k|k-1},y_k-C_k{\hat{x}}_{k|k-1})+K_k\text{cov}(y_k-C_k{\hat{x}}_{k|k-1})K_k^{\mathrm{\top }}\\ & =P_{k|k-1}-K_k{C}_k{P}_{k|k-1}-P_{k|k-1}{C}_k^{\mathrm{\top }}{K}^{\mathrm{\top }}+K_k(C_k{P}_{k|k-1}{C}_k^{\mathrm{\top }}+R_k)K_k^{\mathrm{\top }}\end{array}$$

Kalman Gain Derivation

Optimal Kalman Gain:

$$\frac{\partial \text{tr}(P_{k|k})}{\partial K_k}=-2P_{k|k-1}{C}_k^{\mathrm{\top }}+2K_k(C_k{P}_{k|k-1}{C}_k^{\mathrm{\top }}+R_k)=0$$$$K_k=P_{k|k-1}{C}_k^{\mathrm{\top }}(C_k{P}_{k|k-1}{C}_k^{\mathrm{\top }}+R_k{)}^{-1}$$

Covariance Derivation:

$$P_{k|k}=P_{k|k-1}-K_k{C}_k{P}_{k|k-1}=(P_{k|k-1}^{-1}+C_k^{\mathrm{\top }}{R}_k^{-1}{C}_k{)}^{-1}$$

Probabilistic Perspective: Bayesian Filtering

Bayesian Filtering Framework

$$\begin{array}{rl}& p(x_k|y_{1:k},u_{1:k})\\ =& p(x_k|y_k,y_{1:k-1},u_{1:k})\\ =& \frac{p(y_k|x_k,y_{1:k-1},u_{1:k})\cdot p(x_k|y_{1:k-1},u_{1:k})}{p(y_k|y_{1:k-1},u_{1:k})}\\ =& \eta \cdot p(y_k|x_k)\cdot p(x_k|y_{1:k-1},u_{1:k})\\ =& \eta \cdot p(y_k|x_k)\cdot \int p(x_k,x_{k-1}|y_{1:k-1},u_{1:k})\mathrm{d}{x}_{k-1}\\ =& \eta \cdot p(y_k|x_k)\cdot \int p(x_k|x_{k-1},y_{1:k-1},u_{1:k})\cdot p(x_{k-1}|y_{1:k-1},u_{1:k})\mathrm{d}{x}_{k-1}\\ =& \eta \cdot \underset{\text{observation model}}{\underbrace{p(y_k|x_k)}}\cdot \int \underset{\text{motion model}}{\underbrace{p(x_k|x_{k-1},u_k)}}\cdot \underset{\text{previous belief}}{\underbrace{p(x_{k-1}|y_{1:k-1},u_{1:k-1})}}\mathrm{d}{x}_{k-1}\end{array}$$

Prediction Step: Gaussian Propagation

$$p(x_k|y_{1:k},u_{1:k})=\eta \cdot \mathcal{N}(y_k;C_k{x}_k,R_k)\cdot \int \mathcal{N}(x_k;A_{k-1}{x}_{k-1}+B_{k-1}{u}_{k-1},Q_{k-1})\cdot \mathcal{N}(x_{k-1};{\hat{x}}_{k-1},P_{k-1})\mathrm{d}{x}_{k-1}$$

Predicted Mean:

$$\begin{array}{rl}{\hat{x}}_{k|k-1}& =\mathbb{E}[A_{k-1}{x}_{k-1}+B_{k-1}{u}_{k-1}+w_{k-1}]\\ & =A_{k-1}\mathbb{E}[x_{k-1}]+B_{k-1}{u}_{k-1}+\mathbb{E}[w_{k-1}]\\ & =A_{k-1}{\hat{x}}_{k-1}+B_{k-1}{u}_{k-1}\end{array}$$

Predicted Covariance:

$$\begin{array}{rl}{P}_{k|k-1}& =\text{cov}[A_{k-1}{x}_{k-1}+B_{k-1}{u}_{k-1}+w_{k-1}]\\ & =\text{cov}[A_{k-1}{x}_{k-1}]+\text{cov}[w_{k-1}]\\ & =A_{k-1}\text{cov}[x_{k-1}]A_{k-1}^{\mathrm{\top }}+Q_{k-1}\\ & =A_{k-1}{P}_{k-1}{A}_{k-1}^{\mathrm{\top }}+Q_{k-1}\end{array}$$

Update Step: Gaussian Product

$$p(x_k|y_{1:k},u_{1:k})=\eta \cdot \mathcal{N}(y_k;C_k{x}_k,R_k)\cdot \mathcal{N}(x_k;{\hat{x}}_{k|k-1},P_{k|k-1})$$

Gaussian Product:

$$\mathcal{N}(x;\mu ,\mathrm{\Sigma })\propto \mathcal{N}(x;{\mu }_1,{\mathrm{\Sigma }}_1)\cdot \mathcal{N}(x,{\mu }_2,{\mathrm{\Sigma }}_2)$$$$\begin{array}{rl}{\mathrm{\Sigma }}^{-1}& ={\mathrm{\Sigma }}_1^{-1}+{\mathrm{\Sigma }}_2^{-1}\\ \mu & =\mathrm{\Sigma }({\mathrm{\Sigma }}_1^{-1}{\mu }_1+{\mathrm{\Sigma }}_2^{-1}{\mu }_2)\end{array}$$

Posterior Result:

$$\begin{array}{rl}{\hat{x}}_{k|k}& ={\hat{x}}_{k|k-1}+K_k(y_k-C_k{\hat{x}}_{k|k-1})\\ K_k& =P_{k|k-1}{C}_k^{\mathrm{\top }}(C_k{P}_{k|k-1}{C}_k^{\mathrm{\top }}+R{)}^{-1}\\ P_{k|k}& =(I-K_k{C}_k)P_{k|k-1}\end{array}$$

Optimization Perspective: MAP Estimation

Maximum A Posteriori Formulation

MAP Estimation:

$$\begin{array}{rl}{\hat{x}}_{k|k}& =\arg \underset{x_k}{max}p(x_k\mid y_{1:k})\\ & =\arg \underset{x_k}{min}[-\log p(x_k\mid y_{1:k})]\end{array}$$

Weighted Least Square:

$$\mathcal{E}(x)=||Mx-n|{|}_{\mathrm{\Sigma }}^2=x^{\mathrm{\top }}{M}^{\mathrm{\top }}{\mathrm{\Sigma }}^{-1}Mx-2n^{\mathrm{\top }}{\mathrm{\Sigma }}^{-1}Mx+n^{\mathrm{\top }}{\mathrm{\Sigma }}^{-1}n$$$$\mathrm{\nabla }\mathcal{E}=2M^{\mathrm{\top }}{\mathrm{\Sigma }}^{-1}Mx-2M^{\mathrm{\top }}{\mathrm{\Sigma }}^{-1}n$$$$\hat{x}=(M^{\mathrm{\top }}{\mathrm{\Sigma }}^{-1}M{)}^{-1}{M}^{\mathrm{\top }}{\mathrm{\Sigma }}^{-1}n$$

MAP as Weighted Least Squares

Posterior Distribution:

$$p(x_k\mid y_{1:k})\propto p(y_k\mid x_k)p(x_k\mid y_{1:k-1})$$

Assume Gaussian Distributions:

$$\begin{array}{rl}p(x_k\mid y_{1:k-1})& =\mathcal{N}(x_k;{\hat{x}}_{k|k-1},P_{k|k-1})\\ p(y_k\mid x_k)& =\mathcal{N}(y_k;C_k{x}_k,R_k)\end{array}$$

Negative Log-Posterior:

$$\begin{array}{rl}-\log p(x_k\mid y_{1:k})& \propto \frac{1}{2}\parallel y_k-C_k{x}_k{\parallel }_{R_k^{-1}}^2+\frac{1}{2}\parallel x_k-{\hat{x}}_{k|k-1}{\parallel }_{P_{k|k-1}^{-1}}^2\\ & =\frac{1}{2}{\Vert \left[\begin{array}{c}{C}_k\\ I\end{array}\right]x_k-\left[\begin{array}{c}{y}_k\\ {\hat{x}}_{k|k-1}\end{array}\right]\Vert }_{{\mathrm{\Sigma }}^{-1}}^2\end{array}$$

where $\mathrm{\Sigma }=\left[\begin{array}{cc}{R}_k& 0\\ 0& P_{k|k-1}\end{array}\right]$.

MAP Solution

Weighted Least Squares Form:

$$M=\left[\begin{array}{c}{C}_k\\ I\end{array}\right],n=\left[\begin{array}{c}{y}_k\\ {\hat{x}}_{k|k-1}\end{array}\right],\mathrm{\Sigma }=\left[\begin{array}{cc}{R}_k& 0\\ 0& P_{k|k-1}\end{array}\right]$$

MAP Estimate:

$$\begin{array}{rl}{\hat{x}}_{k|k}& ={\left(M^{\mathrm{\top }}{\mathrm{\Sigma }}^{-1}M\right)}^{-1}{M}^{\mathrm{\top }}{\mathrm{\Sigma }}^{-1}n\\ & ={(C_k^{\mathrm{\top }}{R}_k^{-1}{C}_k+P_{k|k-1}^{-1})}^{-1}(C_k^{\mathrm{\top }}{R}_k^{-1}{y}_k+P_{k|k-1}^{-1}{\hat{x}}_{k|k-1})\end{array}$$

Equivalence Proof

Using Matrix Inversion Lemma:

$$\begin{array}{rl}{\hat{x}}_{k|k}& ={(C_k^{\mathrm{\top }}{R}_k^{-1}{C}_k+P_{k|k-1}^{-1})}^{-1}(C_k^{\mathrm{\top }}{R}_k^{-1}{y}_k+P_{k|k-1}^{-1}{\hat{x}}_{k|k-1})\\ & ={\hat{x}}_{k|k-1}+P_{k|k-1}{C}_k^{\mathrm{\top }}(C_k{P}_{k|k-1}{C}_k^{\mathrm{\top }}+R_k{)}^{-1}(y_k-C_k{\hat{x}}_{k|k-1})\end{array}$$

Proof:

$$\begin{array}{r}{(C_k^{\mathrm{\top }}{R}_k^{-1}{C}_k+P_{k|k-1}^{-1})}^{-1}{C}_k^{\mathrm{\top }}{R}_k^{-1}=P_{k|k-1}{C}_k^{\mathrm{\top }}(C_k{P}_{k|k-1}{C}_k^{\mathrm{\top }}+R_k{)}^{-1}\end{array}$$

This shows the equivalence between the MAP solution and the Kalman update.

Conclusion

Theoretical Insights and Extensions

Key Insights:

• Geometric: Reveals orthogonality principle and innovation process
• Probabilistic: Shows optimality under Gaussian assumptions
• Optimization: Connects to weighted least squares and regularization

Unified Algorithm: All approaches yield the same recursive equations:

$$\begin{array}{rl}\text{time update }& \{\begin{array}{l}{\hat{x}}_{k|k-1}=A_{k-1}{\hat{x}}_{k-1|k-1}\\ P_{k|k-1}=A_{k-1}{P}_{k-1|k-1}{A}_{k-1}^{\mathrm{\top }}+Q\end{array}\\ \text{measurement update }& \{\begin{array}{l}{K}_k=P_{k|k-1}{C}_k^{\mathrm{\top }}(C_k{P}_{k|k-1}{C}_k^{\mathrm{\top }}+R{)}^{-1}\\ {\hat{x}}_{k|k}={\hat{x}}_{k|k-1}+K_k(y_k-C_k{\hat{x}}_{k|k-1})\\ P_{k|k}=(I-K_k{C}_k)P_{k|k-1}\end{array}\end{array}$$

Extensions:

• Nonlinear systems: EKF, UKF, particle filters
• Non-Gaussian noise: robust Kalman filters