拓展卡尔曼滤波EKF

How to fusion two Gauss Distribution
$$\begin{array}{c}\begin{array}{r}\mathcal{N}(x,{\mu }_0,{\sigma }_0)\mathcal{N}(x,{\mu }_1,{\sigma }_1)=\mathcal{N}(x,{\mu }^{\prime },{\sigma }^{\prime })\end{array}\end{array}$$
$$\begin{array}{c}\begin{array}{rl}{\mu }^{\prime }& =\frac{{\mu }_0{\sigma }_1^2+{\mu }_1{\sigma }_0^2}{{\sigma }_0^2+{\sigma }_1^2}={\mu }_0+\frac{{\sigma }_0^2({\mu }_1-{\mu }_0)}{{\sigma }_0^2+{\sigma }_1^2}\\ {\sigma }^{\prime }& =\frac{{\sigma }_0^2{\sigma }_1^2}{{\sigma }_0^2+{\sigma }_1^2}={\sigma }_0^2-\frac{{\sigma }_0^4}{{\sigma }_0^2+{\sigma }_1^2}\end{array}\end{array}$$
$$\begin{array}{c}\begin{array}{rl}k& =\frac{{\sigma }_0^2}{{\sigma }_0^1+{\sigma }_1^2}\\ {\mu }^{\prime }& ={\mu }_0+k({\mu }_1-{\mu }_0)\\ {\sigma }^{\prime }& ={\sigma }_0^2-k{\sigma }_0^2\end{array}\end{array}$$

EFK
根据建模预测：
$$\begin{array}{c}\begin{array}{r}{\hat{x}}_k=F_k{x}_{k-1}+B_k{u}_k\\ {\hat{P}}_k=F_k{P}_{k-1}{F}_k^T+Q_k\end{array}\end{array}$$
将预测转到测量空间：
$$\begin{array}{c}\begin{array}{r}({\mu }_0,{\Sigma }_0)=(H_k{\hat{x}}_k,H_k{\hat{P}}_k{H}_k^T)\end{array}\end{array}$$
测量(measurement)满足：
$$\begin{array}{c}\begin{array}{r}({\mu }_1,{\Sigma }_1)=(z_k^{\prime },R_k)\end{array}\end{array}$$
融合预测和测量：
$$\begin{array}{c}\begin{array}{rl}K& =H_k{\hat{P}}_k{H}_k^T(R_k+H_k{\hat{P}}_k{H}_k^T{)}^{-1}\\ H_k{x}_k& =H_k{\hat{x}}_k+K(z_k^{\prime }-H_k{\hat{x}}_k)\\ H_k{P}_k{H}_k^T& =H_k{\hat{P}}_k{H}_k^T-K{H}_k{\hat{P}}_k{H}_k^T\end{array}\end{array}$$
化简之后：
$$\begin{array}{c}\begin{array}{rl}K& ={\hat{P}}_k{H}_k^T(R_k+H_k{\hat{P}}_k{H}_k^T{)}^{-1}\\ x_k& ={\hat{x}}_k+K^{\prime }(z_k^{\prime }-H_k{\hat{x}}_k)\\ P_k& ={\hat{P}}_k-K^{\prime }{H}_k{\hat{P}}_k\end{array}\end{array}$$
这里的 $K$ 被称为卡尔曼增益。观察（8）中的$x$变化，其中后半部分体现的是观测和预测的差，而卡尔曼增益即为该项的权重。