Kalman Filter

Let us consider a LTI system with additive Gaussian noise $$\begin{array}{rlrl}{x}_k& =A{x}_{k-1}+B{u}_{k-1}+w_{k-1},& Q_k& =\mathbb{E}[w_k{w}_k^{\top }]\\ y_k& =C{x}_k+v_k,& R_k& =\mathbb{E}[v_k{v}_k^{\top }].\end{array}$$ In addition to the state estimate ${\hat{x}}_k$ the state of the Kalman Filter includes the error covariance $$P_k=\mathbb{E}[e_k{e}_k^{\top }],$$ where $e_k={\hat{x}}_k-x_k$ is the state estimate's error.

The Kalman filter estimates the state $x_k$ in two steps, first creating an a priori state estimate and error covariance $$\begin{array}{rl}{\hat{x}}_{k|k-1}& =A{\hat{x}}_{k-1}+B{u}_{k-1}\\ P_{k|k-1}& =A{P}_{k-1}{A}^{\top }+Q_{k-1},\end{array}$$ where $$P_{k|k-1}=\mathbb{E}[e_{k|k-1}{e}_{k|k-1}^{\top }],e_{k|k-1}={\hat{x}}_{k|k-1}-x_k=A{e}_{k-1}-w_{k-1},$$ and then performing a correction based on the measurement $y_k$ and Kalman gain $L_k$, to attain the a posteriori state estimate and error covariance $$\begin{array}{rl}{\hat{x}}_k& ={\hat{x}}_{k|k-1}-L_k(C{\hat{x}}_{k|k-1}-y_k)\\ P_k& =(I-L_{k}C)P_{k|k-1}(I-L_{k}C{)}^{\top }+L_k{R}_k{L}_k^{\top },\end{array}\text{\hspace{1em}(1)}$$ as $$e_k={\hat{x}}_k-x_k=(I-L_{k}C)e_{k|k-1}+L_k{v}_k.$$ The Kalman gain is designed to minimize the a posteriori error covariance by finding the stationary point of (1b) $$L_k=P_{k|k-1}{C}^{\top }{S}_k^{-1},S_k=C{P}_{k|k-1}{C}^{\top }+R_k.\text{\hspace{1em}(2)}$$ After substituting (2) into (1b) we can manipulate the equation into the "Joseph" form $$P_k=P_{k|k-1}-L_k{S}_k{L}_k^{\top },$$ or alternatively the simplified form $$P_k=(I-L_{k}C)P_{k|k-1},$$ which is numerically less stable.