卡尔曼滤波器：4.误差协方差矩阵数学推导

卡尔曼滤波器：4.误差协方差矩阵数学推导

priori / posteriori error covariance matrix(误差协方差矩阵)

回忆一下之前的内容
$$\begin{array}{rl}{x}_k& =A{x}_{k-1}+B{u}_{k-1}+{\omega }_{k-1}\\ z_k& =H{x}_k+{\nu }_k\end{array}$$
则其概率分布$p(\omega )\sim (0,Q)$，其中0代表了其期望，Q代表其协方差矩阵

同样测量噪声也是被认为服从正态分布$p(\nu )\sim (0,R)$和上面的一样

在上面的课程中分别推导出来了三个公式：
$$
\begin{flalign*}
&先验估计：\
&\hat{x}^-k= A\widehat{x}{k-1}+Bu_{k-1}\
&后验估计：\
&\hat{x}_k=\hat{x}^-_k + K_k(z_k - H\hat{x}^-_k)
&其中K_k\in[0,H^-]\
&卡尔曼增益\
&K_k=\frac{P^-_kHT}{HP^-_kHT+R}\
&

\end{flalign*}
$$分析上述式子，只有误差协方差矩阵的先验是未知的？那么这一个环节就是重点讲解\ast \ast 误差协方差先验\ast \ast 的问题$$
\begin{flalign*}
&P^-_k=E[e-ke^{-T}k]\
&e^-k = x_k - \hat{x}^-k\
&=Ax{k-1}+Bu{k-1}+\omega{k-1}-A\hat{x}{k-1}-Bu_{k-1}\
&=A(x_{k-1}-\hat{x}{k-1})+\omega{k-1}
&&其中x_{k-1}-\hat{x}{k-1}=e{k-1}\
&=Ae_{k-1}+\omega_{k-1}\
&P^-_k=E[e-ke^{-T}k]\
&=E[(Ae{k-1}+\omega{k-1})(Ae_{k-1}+\omega_{k-1})^T]\
&=E[(Ae_{k-1}+\omega_{k-1})(e^T_{k-1}AT+\omega_{k-1}^T)]\
&=E[Ae_{k-1}e^T_{k-1}AT+Ae_{k-1}\omega_{k-1}^{T+\omega_{k-1}e}T_{k-1}A^{T+\omega_{k-1}\omega_{k-1}}T]\
&=E(Ae_{k-1}e^T_{k-1}AT)+E(Ae_{k-1}\omega_{k-1}^{T)+E(\omega_{k-1}e}T_{k-1}A^{T)+E(\omega_{k-1}\omega_{k-1}}T)\
&=E(Ae_{k-1}e^T_{k-1}AT)+E(\omega_{k-1}\omega_{k-1}^T)\
&=AE(e_{k-1}e^T_{k-1})AT+E(\omega_{k-1}\omega_{k-1}^T)\
&=AP_{k-1}A^T+Q
&
\end{flalign*}
$$根据上述的推导可以得到：$$
P^{-_k=AP_{k-1}A}T+Q
$$\ast \ast 更新误差协方差矩阵为下一次做准备：\ast \ast$$
\begin{align}
P_k&=P^-_k - K_kHP^-_k - P^-_kHTK_k^T+K_kHP-_kH^TK_kT+K_kRK_k^T\
&=P^-_k - K_kHP^-_k - P^-_kHTK_k^T+K_k(HP-_kH^T+R)KT_k\
&=P^-_k - K_kHP^-_k - P^-_kHTK_k^T+\frac{P-_kH^T}{HP-_kH^T+R}(HP-_kH^T+R)KT_k\
&=P^-_k - K_kHP^-_k - P^-_kHTK_k^T+P-_kH^TKT_k\
&=P^-_k - K_kHP^-_k\
&=(I-K_kH)P^-_k
\end{align}
$$

	预测		更新
先验	${\hat{x}}_k^{-}=A{\hat{x}}_{k-1}+B{u}_{k-1}$	卡尔曼增益	$K_k=\frac{P_k^{-}{H}^T}{H{P}_k^{-}{H}^T+R}$
先验误差协方差	$P_k^{-}=A{P}_{k-1}{A}^T+Q$	后验估计	${\hat{x}}_k={\hat{x}}_k^{-}+K_k(z_k-H{\hat{x}}_k^{-})$
		更新误差协方差	$P_k=(I-K_{k}H)P_k^{-}$

最开始的时候我们要给式子中的${\hat{x}}_{k-1}$和$P_{k-1}$赋初始值${\hat{x}}_0$$P_0$

关于赋初值这个问题放在后面讨论！！