拓展卡尔曼滤波EKF
How to fusion two Gauss Distribution
N ( x , μ 0 , σ 0 ) N ( x , μ 1 , σ 1 ) = N ( x , μ ′ , σ ′ ) \begin{equation} \begin{aligned} \mathcal{N}(x,\mu_0,\sigma_0) \mathcal{N}(x,\mu_1,\sigma_1) = \mathcal{N}(x,\mu',\sigma') \\ \end{aligned} \end{equation} N(x,μ0,σ0)N(x,μ1,σ1)=N(x,μ′,σ′)
μ ′ = μ 0 σ 1 2 + μ 1 σ 0 2 σ 0 2 + σ 1 2 = μ 0 + σ 0 2 ( μ 1 − μ 0 ) σ 0 2 + σ 1 2 σ ′ = σ 0 2 σ 1 2 σ 0 2 + σ 1 2 = σ 0 2 − σ 0 4 σ 0 2 + σ 1 2 \begin{equation} \begin{aligned} \mu' &= \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' &= \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{aligned} \end{equation} μ′σ′=σ02+σ12μ0σ12+μ1σ02=μ0+σ02+σ12σ02(μ1−μ0)=σ02+σ12σ02σ12=σ02−σ02+σ12σ04
k = σ 0 2 σ 0 1 + σ 1 2 μ ′ = μ 0 + k ( μ 1 − μ 0 ) σ ′ = σ 0 2 − k σ 0 2 \begin{equation} \begin{aligned} k &= \frac{\sigma_0^2}{\sigma_0^1 + \sigma_1^2} \\ \mu' &= \mu_0 + k(\mu_1 - \mu_0) \\ \sigma' &= \sigma_0^2 - k\sigma_0^2 \\ \end{aligned} \end{equation} kμ′σ′=σ01+σ12σ02=μ0+k(μ1−μ0)=σ02−kσ02
EFK
根据建模预测：
x ^ k = F k x k − 1 + B k u k P ^ k = F k P k − 1 F k T + Q k \begin{equation} \begin{aligned} \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{aligned} \end{equation} x^k=Fkxk−1+BkukP^k=FkPk−1FkT+Qk
将预测转到测量空间：
( μ 0 , Σ 0 ) = ( H k x ^ k , H k P ^ k H k T ) \begin{equation} \begin{aligned} (\mu_0,\Sigma_0) = (H_k \hat{x}_k, H_k\hat{P}_kH_k^T) \end{aligned} \end{equation} (μ0,Σ0)=(Hkx^k,HkP^kHkT)
测量(measurement)满足：
( μ 1 , Σ 1 ) = ( z k ′ , R k ) \begin{equation} \begin{aligned} (\mu_1,\Sigma_1) = (z_k',R_k) \\ \end{aligned} \end{equation} (μ1,Σ1)=(zk′,Rk)
融合预测和测量：
K = H k P ^ k H k T ( R k + H k P ^ k H k T ) − 1 H k x k = H k x ^ k + K ( z k ′ − H k x ^ k ) H k P k H k T = H k P ^ k H k T − K H k P ^ k H k T \begin{equation} \begin{aligned} K &= H_k\hat{P}_kH_k^T(R_k + H_k\hat{P}_kH_k^T)^{-1} \\ H_kx_k &= H_k\hat{x}_k + K(z_k' - H_k\hat{x}_k) \\ H_kP_kH_k^T &= H_k\hat{P}_kH_k^T - KH_k\hat{P}_kH_k^T \\ \end{aligned} \end{equation} KHkxkHkPkHkT=HkP^kHkT(Rk+HkP^kHkT)−1=Hkx^k+K(zk′−Hkx^k)=HkP^kHkT−KHkP^kHkT
化简之后：
K = P ^ k H k T ( R k + H k P ^ k H k T ) − 1 x k = x ^ k + K ′ ( z k ′ − H k x ^ k ) P k = P ^ k − K ′ H k P ^ k \begin{equation} \begin{aligned} K &= \hat{P}_kH_k^T(R_k + H_k\hat{P}_kH_k^T)^{-1} \\ x_k &= \hat{x}_k + K'(z_k' - H_k\hat{x}_k) \\ P_k &= \hat{P}_k - K'H_k\hat{P}_k \\ \end{aligned} \end{equation} KxkPk=P^kHkT(Rk+HkP^kHkT)−1=x^k+K′(zk′−Hkx^k)=P^k−K′HkP^k
这里的 K K K 被称为卡尔曼增益。观察（8）中的 x x x变化，其中后半部分体现的是观测和预测的差，而卡尔曼增益即为该项的权重。