Kalman filtering

Assume we have computed ${\hat{\mathbf{x}}}_i\triangleq {\hat{\mathbf{x}}}_{i|i-1}$ and we get a new measurement ${\mathbf{y}}_i$. Then $${\hat{\mathbf{x}}}_{i|i}={\hat{\mathbf{x}}}_i+{\mathbf{K}}_{f,i}{\mathbf{e}}_i\mathsf{\text{ with }}{\mathbf{K}}_{f,i}={\mathbf{P}}_i{\mathbf{H}}_i^T{\Vert {\mathbf{e}}_i\Vert }_{}^{-2}$$ $${\mathbf{P}}_{i|i}\triangleq {\Vert {\mathbf{x}}_i-{\hat{\mathbf{x}}}_{i|i}\Vert }_{}^2={\mathbf{P}}_i-{\mathbf{P}}_i{\mathbf{H}}_i^T{\Vert {\mathbf{e}}_i\Vert }_{}^{-2}{\mathbf{H}}_i{\mathbf{P}}_i$$

Assume we have computed ${\hat{\mathbf{x}}}_{i|i}$ and ${\mathbf{P}}_{i|i}$. Then, $${\hat{\mathbf{x}}}_{i+1}={\mathbf{F}}_i{\hat{\mathbf{x}}}_{i|i}+{\mathbf{G}}_i{\hat{\mathbf{u}}}_{i|i}\mathsf{\text{ with }}{\hat{\mathbf{u}}}_{i|i}={\mathbf{S}}_i{\Vert {\mathbf{e}}_i\Vert }_{}^{-2}{\mathbf{e}}_i$$ $${\mathbf{P}}_{i+1}={\mathbf{F}}_i{\mathbf{P}}_{i|i}{\mathbf{F}}_i^T+{\mathbf{G}}_i({\mathbf{Q}}_i-{\mathbf{S}}_i{\Vert {\mathbf{e}}_i\Vert }_{}^2{\mathbf{S}}_i^T){\mathbf{G}}_i^T-{\mathbf{F}}_i{\mathbf{K}}_{f,i}{\mathbf{S}}_i^T{\mathbf{G}}_i^T-{\mathbf{G}}_i{\mathbf{S}}_i{\mathbf{K}}_{f,i}^T{\mathbf{F}}_i^T$$

If ${\mathbf{R}}_i\succ 0$ then ${\Vert {\mathbf{e}}_i\Vert }_{}^2\succ 0$

If ${\mathbf{R}}_i\succ 0$, define ${\mathbf{u}}_{i,s}\triangleq {\mathbf{u}}_i-{\mathbf{S}}_i{\mathbf{R}}_i^{-1}{\mathbf{v}}_i$. Then, $${\mathbf{P}}_{i+1}={\mathbf{F}}_{i,s}{\mathbf{P}}_{i|i}{\mathbf{F}}_{i,s}^T+{\mathbf{G}}_i{\mathbf{Q}}_{i,s}{\mathbf{G}}_i^T$$ where ${\mathbf{F}}_{i,s}\triangleq {\mathbf{F}}_i-{\mathbf{G}}_i{\mathbf{S}}_i{\mathbf{R}}_i^{-1}{\mathbf{H}}_i$ and ${\mathbf{Q}}_{i,s}={\mathbf{Q}}_i-{\mathbf{S}}_i{\mathbf{R}}_i^{-1}{\mathbf{S}}_i^T$

If ${\mathbf{R}}_i\succ 0$ and ${\mathbf{P}}_i\succ 0$ then ${\mathbf{P}}_{i|i}^{-1}={\mathbf{P}}_i^{-1}+{\mathbf{H}}_i^T{\mathbf{R}}_i^{-1}{\mathbf{H}}_i$ and $${\mathbf{P}}_{i|i}^{-1}{\hat{\mathbf{x}}}_{i|i}={\mathbf{P}}_{i-1}{\hat{\mathbf{x}}}_i+{\mathbf{H}}_i^T{\mathbf{R}}_i^{-1}{\mathbf{y}}_i$$