卡尔曼滤波器：5.直观理解和二维实例

卡尔曼滤波器：5.直观理解和二维实例.md

[2D Kalman Filter Example_Dr_CAN.xlsx](2D Kalman Filter Example_Dr_CAN.xlsx)

上面的这个例子是：

一个人在走路：匀速运动

状态估计

状态 ：$x_1$位置 $x_2$速度

位置：$x_{1,k}=x_{1,k-1}+\Delta T{x}_{2,k-1}$

速度：$x_{2,k}=x_{2,k-1}$

采样时间$\Delta T$ $k$时刻与$k-1$时刻的间隔

取$\Delta T=1$且在行走中存在着不确定性

位置：$x_{1,k}=x_{1,k-1}+x_{2,k-1}+{\omega }_{1,k-1}$

速度：$x_{2,k}=x_{2,k-1}+{\omega }_{2,k-1}$

其中$p(\omega )\sim N(0,Q)$

测量方程

还有一个就是放置卫星来检测行人的位置和速度 增加不确定性$\nu$ $p(\nu )\sim N(0,R)$
KaTeX parse error: No such environment: flalign* at position 8: \begin{̲f̲l̲a̲l̲i̲g̲n̲*̲}̲ &z_{1,k} = x_{…

用矩阵的形式表达出来

$$
\begin{flalign*}
&\left[\matrix{x_{1,k}\x_{2,k}}\right] = \left[\matrix{1&1\0&1}\right]\left[\matrix{x_{1,k-1}\x_{2,k-1}}\right]+\left[\matrix{\omega_{1,k-1}\\omega_{2,k-1}}\right]\
&\left[\matrix{z_{1,k}\z_{2,k}}\right] = \left[\matrix{1&0\0&1}\right]\left[\matrix{x_{1,k}\x_{2,k}}\right]+\left[\matrix{\nu_{1,k-1}\\nu_{2,k-1}}\right]
&

\end{flalign*}
$$

	预测		更新
先验	${\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^{-}$