卡尔曼滤波算法

卡尔曼滤波算法

非常喜欢这个配图，先贴出来出处：http://www.bzarg.com/p/how-a-kalman-filter-works-in-pictures/

卡尔曼滤波的五个公式

卡尔曼滤波主要分为两部分：预测(prediction) 和 更新(update), 也叫作 时间更新 和 量测更新 二者可以不同步

这是因为时间更新是根据数学模型递推得到的，它可以不依赖于观测。但是由于系统存在噪声，数学模型在经过一段时间以后可能会出现偏差，这是我们想到可以用观测来矫正。类似于加权算法，卡尔曼滤波通过卡尔曼增益对模型推测和观测之间进行动态修正。

举个例子，一个GPS导航系统，要求实时性保证，主系统的时间更新是更频繁的。但是由于GPS更新速率大概在20hz左右，所以只能与主系统分开更新。卡尔曼滤波正好能实现这个要求。

系统模型：

考虑线性系统，设$t_k$时刻被估计状态$X_k$时候系统噪声$W_k$影响。已知前向估计$\{{\hat{x}}_{k-1},{\hat{P}}_{k-1}\}$, 根据马尔可夫性，${\hat{x}}_k$与${\hat{x}}_{k-1}$之前状态无关，则：
$$\begin{gathered} 状态转移方程：X_k={\Phi }_{k,k-1}{X}_{k-1}+W_k \\ 量测方程：Z_k=H_k{X}_k+V_k \end{gathered}$$$$\begin{gathered} 符号含义： \\ {X}_k是t_k时刻的状态，X_{k-1}是t_{k-1}时刻的状态； \end{gathered}$$

$$\begin{gathered} Z_k是t_k时刻的量测值，H_k是状态-量测转移矩阵 \\ {W}_k是过程噪声，V_k是量测噪声； \\ {W}_k\sim N(0,Q_k),Q_k是过程噪声的协方差矩阵； \\ {V}_k\sim N(0,R_k),R_k是量测噪声的协方差矩阵； \end{gathered}$$

时间更新：（prediction）

$$\begin{gathered} 状态一步预测：{\check{X}}_{k/k-1}={\Phi }_{k,k-1}{\hat{X}}_{k-1}① \\ 一步预测均方误差：{\check{P}}_{k/k-1}={\Phi }_{k,k-1}{\hat{P}}_{k-1}{\Phi }_{k,k-1}^T+Q_k② \end{gathered}$$
$$\begin{gathered} 符号含义： \\ {\check{X}}_{k/k-1}是t_{k-1}时刻到t_k时刻的状态先验值，也就是根据上一时刻的值模型推测出来的估计值； \end{gathered}$$
$$\begin{gathered} {\hat{X}}_{k-1}是t_{k-1}时刻估计出来的状态，在当前t_k时刻，{\hat{X}}_{k-1}是确定的量； \\ {\Phi }_{k,k-1}是t_{k-1}时刻到t_k时刻的一步转移矩阵； \\ {\check{P}}_{k/k-1}是{\check{X}}_{k/k-1}的均方误差，代表模型预测的均方误差； \\ {\hat{P}}_{k-1}是{\hat{X}}_{k-1}的均方误差，在当前t_k时刻，{\hat{P}}_{k-1}是确定的量； \end{gathered}$$

量测更新：（update）

$$\begin{gathered} 卡尔曼增益：K_k={\check{P}}_{k/k-1}{H}_k^T(H_k{\check{P}}_{k/k-1}{H}_k^T+R_k{)}^{-1}③ \\ 对t_k时刻的状态估计：{\hat{X}}_k={\check{X}}_{k/k-1}+K_k(Z_k-H_k{\check{X}}_{k/k-1})④ \\ 计算t_k时刻估计的均方误差：{\hat{P}}_k=(I-K_k{H}_k){\check{P}}_{k/k-1}⑤ \end{gathered}$$$$\begin{gathered} 符号含义： \\ {K}_k是t_k时刻卡尔曼增益矩阵，也可以记作K_k=\frac{{\check{P}}_{k/k-1}{H}_k^T}{H_k{\check{P}}_{k/k-1}{H}_k^T+R_k} \\ 可以发现，K_k可以通过数学模型估计方差和量测噪声方差之间的关系动态调整。 \\ 当H_k{\check{P}}_{k/k-1}{H}_k^T>R_k时，偏向传感器的测量值； \\ 当H_k{\check{P}}_{k/k-1}{H}_k^T<R_k时，偏向模型的估计值； \\ {H}_k是状态-量测转移矩阵,代表了状态到观测的映射。 \end{gathered}$$

$$\begin{gathered} {\hat{X}}_k是根据量测Z_k，计算出后验(估计值) \\ {\hat{P}}_k是模型+量测估计的均方误差，可以写作{\hat{P}}_k=(H_k^T{Q}_k{H}_k+{\check{P}}_{k/k-1}^{-1}{)}^{-1}, \\ 根据Sherman-Morrison公式，则：{\hat{P}}_k=(I-K_k{H}_k){\check{P}}_{k/k-1} \end{gathered}$$