卡尔曼滤波推导

##概述
推导卡尔曼滤波算法中的卡尔曼增益表达式
##模型
状态方程：
$$x_{k+1}={\Phi }_{k+1}{x}_k+w_k$$
测量方程：
$$z_k=H_k{x}_k+v_k$$
并假定：
E [ x 0 ] = μ 0 x E [ w k ] = 0   ∀ k E [ v k ] = 0   ∀ k c o v { w k , w j } = Q k δ k j c o v { v k , v j } = R k δ k j c o v { x 0 , x 0 } = P 0 c o v { w k , v j } = 0   ∀ k c o v { x 0 , w k } = 0   ∀ k c o v { x 0 , v j } = 0   ∀ k \begin{aligned} \\ & E[x_0] = \mu_0^x\\ & E[w_k] =0\ \forall k \\ & E[v_k] =0\ \forall k \\ & cov\{w_k,w_j\}=Q_k\delta_{kj}\\ & cov\{v_k,v_j\}=R_k\delta_{kj}\\ & cov\{x_0,x_0\}=P_0\\ & cov\{w_k,v_j\}=0 \ \forall k\\ & cov\{x_0,w_k\}=0 \ \forall k\\ & cov\{x_0,v_j\}=0 \ \forall k\\ \end{aligned} \\ ​E[x0​]=μ0x​E[wk​]=0 ∀kE[vk​]=0 ∀kcov{wk​,wj​}=Qk​δkj​cov{vk​,vj​}=Rk​δkj​cov{x0​,x0​}=P0​cov{wk​,vj​}=0 ∀kcov{x0​,wk​}=0 ∀kcov{x0​,vj​}=0 ∀k​
##推导
根据上述条件将推导以下方程：

1. 状态推断
   $${\hat{x}}_{k+1}^{-}={\Phi }_{k+1}{\hat{x}}_k$$
2. 协方差推断
   $$P_{k+1}^{-}={\Phi }_{k+1}{P}_k{\Phi }_{k+1}^T+Q_k$$
3. 卡尔曼增益计算
   $$K_{k+1}=P_{k+1}^{-}{H}_{k+1}^T(H_{k+1}{P}_{k+1}^{-}{H}_{k+1}^T+R_{k+1}{)}^{-1}$$
4. 状态更新
   $${\hat{x}}_{k+1}={\hat{x}}_{k+1}^{-}+K_{k+1}(z_{k+1}-H_{k+1}{\hat{x}}_{k+1}^{-})$$
5. 协方差更新
   $$P_{k+1}=P_{k+1}^{-}-K_{k+1}{H}_{k+1}{P}_{k+1}^{-}$$
   令：在k+1步可以得到一个k步的无偏差估计，误差项${\tilde{x}}_k={\hat{x}}_k-x_k$的均值为0，协方差为$P_k$；在k步时的测量值$z_k$为: $$z_k=H_k{x}_k+v_k$$
   下面尝试使${\hat{x}}_{k+1}$和${\hat{x}}_k$以如下形式建立联系
   $${\hat{x}}_{k+1}=K_{k+1}^{\prime }{\hat{x}}_k+K_{k+1}{z}_{k+1}$$

###Step.1
由式${\hat{x}}_{k+1}=K_{k+1}^{\prime }{\hat{x}}_k+K_{k+1}{z}_{k+1}$得：
$${\hat{x}}_{k+1}-x_{k+1}=K_{k+1}^{\prime }{\hat{x}}_k+K_{k+1}{z}_{k+1}-x_{k+1}$$
将式$z_k=H_k{x}_k+v_k$ 带入上式得：
$$\begin{array}{rl}{\hat{x}}_{k+1}-x_{k+1}& \\ & =K_{k+1}^{\prime }{\hat{x}}_k+K_{k+1}{H}_{k+1}{x}_{k+1}+v_{k+1}-x_{k+1}\\ & =K_{k+1}^{\prime }{\hat{x}}_k+K_{k+1}{H}_{k+1}{x}_{k+1}+v_{k+1}-x_{k+1}-K_{k+1}^{\prime }{x}_k+K_{k+1}^{\prime }{x}_k\end{array}$$
化简得：

$$\begin{array}{rl}{\hat{x}}_{k+1}-x_{k+1}& \\ =& K_{k+1}^{\prime }({\hat{x}}_k-x_k)+(K_{k+1}{H}_{k+1}{\Phi }_{k+1}-{\Phi }_{k+1}+K_{k+1}^{\prime })x_k\\ & +(K_{k+1}{H}_{k+1}-I)w_{k+1}+K_{k+1}{v}_k\end{array}$$

由$E[{\hat{x}}_{k+1}-x_{k+1}]=0$易知：
$$E[{\hat{x}}_{k+1}-x_{k+1}]=[K_{k+1}{H}_{k+1}{\Phi }_{k+1}-{\Phi }_{k+1}+K_{k+1}^{\prime }]E[x_k]=0$$
$$K_{k+1}{H}_{k+1}{\Phi }_{k+1}-{\Phi }_{k+1}+K_{k+1}^{\prime }=0$$
表明：
$$K_{k+1}^{\prime }=(I-K_{k+1}{H}_{k+1}){\Phi }_{k+1}$$
结论：
$${\hat{x}}_{k+1}=(I-K_{k+1}{H}_{k+1}){\Phi }_{k+1}{\hat{x}}_k+K_{k+1}{z}_{k+1}$$
或：
$${\hat{x}}_{k+1}={\Phi }_{k+1}{\hat{x}}_k+K_{k+1}(z_{k+1}-H_{k+1}{\Phi }_{k+1}{\hat{x}}_k)$$

###Step.2
建立使误差项${\tilde{x}}_{k+1}={\hat{x}}_{k+1}-x_{k+1}$的协方差矩阵$P_{k+1}$的迹最小的$K_{k+1}$的表达式
####A.
由${\tilde{x}}_{k+1}^{-}={\hat{x}}_{k+1}^{-}-x_{k+1}$：
$${\tilde{x}}_{k+1}^{-}={\hat{x}}_{k+1}^{-}-x_{k+1}={\Phi }_{k+1}{\hat{x}}_k-{\Phi }_{k+1}{x}_k+w_{k+1}$$
$${\tilde{x}}_{k+1}^{-}={\hat{x}}_{k+1}^{-}-x_{k+1}={\Phi }_{k+1}({\hat{x}}_k-x_k)+w_{k+1}$$
式中${\hat{x}}_{k+1}^{-}-x_{k+1}$的协方差为$P_{k+1}^{-}$，${\hat{x}}_k-x_k$的协方差为$P_k$，$w_{k+1}$的协方差为$Q_{k+1}$。
则可推出方程2
P k + 1 − = E { x ~ k + 1 − x ~ k + 1 − T } = Φ k + 1 E { ( x ~ k − x k ) ( x ~ k − x k ) T } Φ k + 1 T + E { w k + 1 w k + 1 T } = Φ k + 1 P k Φ k + 1 T + Q k + 1 \begin{aligned}\\ P_{k+1}^- &=E\{\widetilde{x}_{k+1}^-\widetilde{x}_{k+1}^{-T}\} \\ &=\Phi_{k+1}E\{(\widetilde{x}_k-x_k)(\widetilde{x}_k-x_k)^T\}\Phi_{k+1}^T+E\{w_{k+1}w_{k+1}^T\}\\ &=\Phi_{k+1} P_k\Phi_{k+1}^T+Q_{k+1}\\ \\ \end{aligned} Pk+1−​​=E{x k+1−​x k+1−T​}=Φk+1​E{(x k​−xk​)(x k​−xk​)T}Φk+1T​+E{wk+1​wk+1T​}=Φk+1​Pk​Φk+1T​+Qk+1​​
####B.
由Step.1中：
$${\hat{x}}_{k+1}={\Phi }_{k+1}{\hat{x}}_k+K_{k+1}(z_{k+1}-H_{k+1}{\Phi }_{k+1}{\hat{x}}_k)$$
和$ \hat{x}{k+1}^- = \Phi{k+1}\hat{x}_k$得：
$${\hat{x}}_{k+1}={\hat{x}}_{k+1}^{-}+K_{k+1}(z_{k+1}-H_{k+1}{\hat{x}}_{k+1}^{-})$$
带入${\tilde{x}}_{k+1}={\hat{x}}_{k+1}-x_{k+1}$中化简得：
$${\tilde{x}}_{k+1}=[I-K_{k+1}{H}_{k+1}]{\tilde{x}}_{k+1}^{-}-K_{k+1}{v}_{k+1}$$
等式两端同时取期望：
$$\begin{gathered} \begin{array}{rl} \\ {P}_{k+1}& \\ & =E[{\tilde{x}}_{k+1}{\tilde{x}}_{k+1}^T]\\ & =[I-K_{k+1}{H}_{k+1}]P_{k+1}^{-}[I-K_{k+1}{H}_{k+1}{]}^T+K_{k+1}{R}_{k+1}{K}_{k+1}^T\end{array} \end{gathered}$$
####C.
推导$K_{k+1}$的表达式使$P_{k+1}$的迹最小
使用$P$代替$P_{k+1}^{-}$,$K$代替$K_{k+1}$,$R$代替$R_{k+1}$得:
$$P_{k+1}=(I-KH)P(I-KH{)}^T+KR{K}^T$$
$$P_{k+1}=P-KHP-P{H}^T{K}^T+KHP{H}^T+KR{K}^T$$
由$P=P^T$，$(P{H}^T{K}^T{)}^T=KHP$，$Tr(P{H}^T{K}^T)=Tr(KHP)$:
$$Tr{P}_{k+1}=TrP-2TrKHP+TrK(HP{H}^T)K^T+TrKR{K}^T$$
$$\frac{\delta Tr{P}_{k+1}}{\delta K}=-2P{H}^T+2KHP{H}^T+2K$$
令$\frac{\delta Tr{P}_{k+1}}{\delta K}$为零最后推出卡尔曼增益$K$的表达式
$$K=P{H}^T(HP{H}^T+R{)}^{-1}$$
可证明达到的极值为最小值(reference Gelb, pg. 109)
最后简化下式$P_{k+1}$，将$K$带入$P_{k+1}$表达式中可推出：
$$P_{k+1}=(I-K_{k+1}{H}_{k+1})P_{k+1}^{-}$$