kalman滤波

Kalman滤波器是一种用递归方法解决离散数据线性滤波问题的滤波器。得益于数 字信号处理技术和集成电路工艺的发展，kalman滤波器的应用场景日益广泛，在 通信系统的各个领域都有所应用。

kalman滤波器用于估计离散事件过程的状态变\(x\in \mathcal{R}^{n}\)。这个 离散时间过程可以用一下离散随机差分方程描述：

\begin{equation} \label{eq:1} x_{k} = Ax_{k-1} + B u_{k-1} + w_{k-1} \end{equation}

另外对于观测系统，我们也需要定义一个观测方程：

\begin{equation} \label{eq:2} z_{k} = Hx_{k} + v_{k} \end{equation}

随机信号\(w_{k}\) 和\(v_{k}\) 分别表示过程噪声和观测噪声。通常假设这两 个随机变量相互独立，并且服从正态分布：

\begin{eqnarray*} p(w)&\sim &N(0,Q) \\ p(v)&\sim &N(0,R) \end{eqnarray*}

在实际系统中，过程噪声协方差矩阵\(Q\)和观测噪声协方差矩阵\(R\)可能会随 着每次迭代计算而变化。

定义\(\hat{x}_{k}^{-} \in \mathcal{R}^{n}\)为一直第\(k\)步以前状态情况 下第\(k\)步的香烟估计。定义\(\hat{x}_{k}\in \mathcal{R}^{n}\)为一直测 量变量\(z_{k}\)时第\(k\)步的后验状态估计。由此定义先验估计误差和后验估 计误差：

\begin{eqnarray*} e_{k}^{-}&=& x_{k} - \hat{x}_{k}^{-} \\ e_{k} &=& x_{k} - \hat{x}_{k} \end{eqnarray*}

先验估计误差的协方差为：

\begin{equation} \label{eq:4} P_{k}^{-} = E[e_{k}^{-}e_{k}^{-T}] \end{equation}

后验估计的协方差为：

\begin{equation} \label{eq:5} R_{k} = E[e_{k}e_{k}^{T}] \end{equation}

我们用先验估计\(\hat{x}_{k}^{-}\)和加权的测量变量\(z_{k}\)以及预测 \(H\hat{x}_{k}^{-}\)之差的线性组合构成了后验状态估计\(\hat{x}_{k}\)。 即：

\begin{equation} \label{eq:6} \hat{x}_{k} = \hat{x}_{k}^{-} + K (z_{k} - H\hat{x}_{k}^{-}) \end{equation}

式~(\ref{eq:6}) 构成了kalman滤波器的表达式。式中的\(z_{k} - H\hat{x}_{k}^{-}\)被称为测量过程的更新或者残余。这个量反映了预测值和实际值之间 的不一致程度。更新为0说明二者完全相同。式中\(K\)叫做更新的增益，\(K\) 的作用是使式~(\ref{eq:5})的后验估计误差协方差尽可能的小。可以把 式~(\ref{eq:6})带入式~(\ref{eq:5})，求得期望之后，将式~(\ref{eq:5})对 \(K\)求导，可得\(K\):

\begin{equation} \label{eq:7} K_{k} = P_{k}^{-}H^{T} (HP_{k}^{-}H^{T} + R)^{-1} \end{equation}

从式~(\ref{eq:7})可知，观测噪声协方差\(R\)越小，更新的增益 \(K\)越小。特别地，\(P_{k}^{-}\)趋向于零时，有：

\begin{equation} \label{eq:8} \lim_{R_{k}\to 0}K_{k} = H^{-1} \end{equation}

从式~(\ref{eq:6})可以看出，\(K_{k} = H^{-1}\)时，有\(\hat{x}_{k} = x_{k}\)，即后验估计值和真实值相等。也就是说当观测方程的误差越来越小的 时候，后验估计值越来越接近真实值。也就是说随着测量噪声越来越小，我们需 要越来越相信测量到的\(z_{k}\)，而不是基于测量方程的预测 \(H\hat{x}_{k}^{-}\)。

另一方面，先验估计误差协方差\(P_{k}^{-}\)越小，残余的增益\(K\)越小。特 别的，\(P_{k}^{-}\)趋于零时，有：

\begin{equation} \label{eq:9} \lim_{P_{k}^{-}\to 0} K_{k} = 0 \end{equation}

这说明先验估计很准的时候，后验估计要和先验估计保持一致。也就是说系统方 程的误差越来越小，我们要相信系统方程做出的先验估计，而不是相信测量变量 \(z_{k}\)。最终的结果都要通过测量方程来输出。根据先验估计误差和测量误 差的变化，我们选择相信测量值还是测量方程的估计值。

kalman滤波用反馈的方式估计过程状态：滤波器估计过程某一时刻的状态，然后 以（含噪声）测量变量的方式获得反馈。kalman滤波器的精髓就在这里。具体体 现在先验估计协方差\(P_{k}^{-}\)和观测噪声的博弈。这是个此消彼长的过程： 系统输出比较精确我就倾向于使用系统的输出；观测是比较精确的我就倾向于使 用观测的结果。

kalman滤波器分两部分：1. 时间更新方程；2. 测量更新方程。时间更新方程负 责向前推算当前状态变量和误差协方差估计，为下一时刻构建先验估计做准备。 测量更新方程负责反馈，即把先验估计和新的测量变量结合以构造改进的后验估 计。所以时间更新方程是预估方程，测量更新方程是校正方程。这两个方程互相 迭代，不断提高后验估计的精度。

基于之前定义的变量，我们在这里不加推导的给出时间更新方程和测量方程对应 的内容。

离散kalman滤波器时间更新方程为：

\begin{eqnarray*} \hat{x}_{k}^{-}&=&A\hat{x}_{k-1} + Bu_{k-1} \\ P_{k}^{-} &=& AP_{k-1}A^{T} + Q \end{eqnarray*}

时间更新方程将状态估计\(x_{k}^{-}\)和协方差估计\(P_{k}^{-}\)从\(k-1\) 时刻向前推算到\(k\)时刻。\(A\)和\(B\) 来自~(\ref{eq:1})， \(Q\)是过程 噪声方差。

离散kalman滤波器状态更新方程为：

\begin{eqnarray*} K_{k}&=&P_{k}^{-}H^{T}(HP_{k}^{-}H^{T} +R )^{-1} \\ \hat{x}_{k} &=& \hat{x}_{k}^{-} + K_{k}(z_{k} - H\hat{x}_{k}^{-}) \\ P_{k} &=& (I - K_{k}H)P_{k}^{-} \end{eqnarray*}

测量更新方程首先要做的是计算kalman增益\(K_{k}\)。然后更新测量输出获得 \(z_{k}\)，更新后验估计\(\hat{x_{k}}\)。最后估计状态的后验协方差。

Kalman滤波器是一种迭代滤波器，在时间更新方程和状态更新方程之间迭代。上 一次得到的后验估计被作为下一次计算的先验估计。这种递推是kalman滤波器的 亮点。它比维也纳滤波器计算量更低。因为维也纳滤波必须计算全部数据，而 kalman滤波器每次只根据以前的测量变量递归计算当前的状态估计。

在实际实现时，测量噪声协方差\(R\)一般可以观测得到，是滤波器的已知条件。 观测测量噪声协方差\(R\)一般是可以实现的。因此我们通常我们离线获取一些 系统观测值以计算测量噪声协方差。

通常更难确定过程激励噪声协方差的\(Q\)值，因此我们无法直接观测到过程信 号\(x_{k}\)。有时可以通过\(Q\)的选择给过程信号注入足够的不确定性来建立 一个简单的过程模型而产生可以接受的结果。在这两种情况下，不管我们是否有 一个合理的标准来选择系数，我们通常都可以通过调整滤波器系数来获得更好的 性能。调整的过程是离线完成的，并经常与另一个确定无误的在线滤波器对比。 这个过程称为系统识别。这里给了机器学习一个机会。

有时候我们假定\(Q\)和\(R\)都是常数，过程估计误差协方差\(R\)和kalman增 益\(K_{k}\)都会快速收敛并保持为常量。若实际情况如此，那么滤波器系数便 可以通过预先离线运行滤波器计算。然而，在实际应用中，观测误差\(R\)尤其 不易保持不变。不仅观测噪声会发生变化，有时候过程激励噪声协方差\(Q\) 也 会随着滤波器运行而发生变化，这样\(Q\)就是时变的了。

在前面两节，我们说明了kalman滤波器原理和迭代过程，在这一节，我们跟进一 步的细化这个过程，并辅以例子阐述几乎每个细节。毕竟，就一个主题进行深入 的逐步的解读才是我的风格。

在这个章节，我们准备分四步来完成对一个动态系统的预测：

1. 用数学语言描述一个动态系统，然后估计这个动态系统的状态；
2. 我们推导这个动态系统的状态均值和方差的变化；
3. 我们使用计算机模拟这个系统；
4. 每次我们获得一个测量，就更新这个系统状态的均值和方差。

在这一节我们描述的动态系统如下：

\begin{eqnarray} \label{eq:3} x_{k}&=& F_{k-1}x_{k-1} + G_{k-1}u_{k-1} + w_{k-1} \\ y_{k}&=& H_{k}x_{k} + v_{k} \end{eqnarray}

其中\(w_{k}\)和\(v_{k}\)是零均值，白高斯噪声：

\begin{eqnarray} \label{eq:10} w_{k}&\sim & N(0,Q_{k}) \\ v_{k}&\sim & N(0,R_{k}) \end{eqnarray}

我们的目的是基于我们对系统的了解和观测值\(y_{k}\) 估计 \(x_{k}\)。我们 的已知信息与要考虑的动态系统密切相关。如果我们有系统截止到（包含） \(k\)时刻的所有观测信息，我们可以获得一个对\(x_{k}\)的后验估计 \(\hat{x}_{k}^{+}\)。我们用\(+\)表示后验估计。获取\(\hat{x}_{k}^{+}\)的 方法是计算后验概率：

\begin{equation} \label{eq:11} \hat{x}_{k}^{+} = E[x_{k}|y_{1},y_{2},\ldots,y_{k}] \end{equation}

如果我们有系统截止到（不包含）\(k\)时刻的所有观测值，我们可以获得一个 对\(x_{k}\)的先验估计值\(\hat{x}_{k}^{-}\)，我们用\(-\)表示先验估计。 即：

\begin{equation} \label{eq:12} \hat{x}_{k}^{-} = E[x_{k} | y_{1},y_{2},\ldots,y_{k-1}] \end{equation}

需要注意的是：\(\hat{x}_{k}^{-}\)和\(\hat{x}_{k}^{+}\)是我们对同一状态 的两种估计。前者基于我们没有获得时刻\(k\)的观测值\(y_{k}\)；后者基于我 们获得了时刻\(k\)的观测值\(y_{k}\)。自然，我们期望\(\hat{x}_{k}^{+}\) 要比\(\hat{x}_{k}^{-}\)要准确。

如果我们有时刻\(k\)之后的一些观测，基于这些观测，我们要对\(x_{k}\)做一 个估计，我们把这个估计叫做平滑估计，用数学描述就是：

\begin{equation} \label{eq:13} \hat{x}_{k|k+N} = E[x_{k}| y_{1},y_{2},\ldots,y_{k},y_{k+1},\ldots,y_{k+N}] \end{equation}

\(N\)是正整数。

如果我们相基于现有观测值对未来某个时刻进行估计，那么我们有一个预测估计：

\begin{equation} \label{eq:14} \hat{x}_{k|k-M} = E[x_{k} | y_{1},y_{2},\ldots,y_{k-M}] \end{equation}

\(M\)是正整数。

对于初始时刻，没有任何观测值，我们用\(\hat{x}_{0}^{+}\)代表对\(x_{0}\) 的一个初始估计。第一个观测值\(y_{1}\)在\(k=1\)时到来，因为我们没有对 \(x_{0}\)的任何观测值，所以一个合理的做法是：

\begin{equation} \label{eq:15} \hat{x}_{0}^{+} = E(x_{0}) \end{equation}

我们用\(P_{k}\)来表示估计误差的协方差矩阵。\(P_{k}^{-}\)表示 \(\hat{x}_{k}^{-}\)的估计误差的协方差矩阵； \(P_{k}^{+}\)表示 \(\hat{x}_{k}^{+}\)的估计误差的协方差矩阵。即：

\begin{eqnarray} \label{eq:16} P_{k}^{-}&=&E[(x_{k} - x_{k}^{-})(x_{k} - x_{k}^{-})^{T}] \\ P_{k}^{+}&=&E[(x_{k} - x_{k}^{+})(x_{k} - x_{k}^{+})^{T}] \end{eqnarray}

在我们收到\(y_{k}\)之前我们有对\(x_{k}\)的一个估计\(\hat{x}_{k}^{-}\) 以及这个估计的误差的协方差举着\(P_{k}^{-}\)。在时刻\(k\)，我们收到了 \(y_{k}\)，此时我们有一个对\(x_{k}\)的估计\(x_{k}^{+}\)和这个估计的误 差的协方差矩阵\(P_{k}^{+}\)

我们从时刻\(0\)开始，此时我们想有一个对\(x_{1}\)状态的先验估计，即：

\begin{equation} \label{eq:17} \hat{x}_{1}^{-} = F_{0}x_{0} + G_{0}u_{0} \end{equation}

这是一个初始估计，可以推而广之：

\begin{equation} \label{eq:18} \hat{x}_{k}^{-} = F_{k-1}x_{k-1}^{+} + G_{k-1}u_{k-1} \end{equation}

这个是\(\hat{x}\)的时间更新方程。接下来我们推导估计误差的协方差矩阵更 新过程，同样我们从\(P_{0}^{+}\)开始：

\begin{equation} \label{eq:19} P_{0}^{+} = E[(x_{0} - \hat{x}_{0}^{+})(x_{0} - \hat{x}_{0}^{+})^{T}] \end{equation}

给定了\(p_{0}^{+}\)，如何得到\(P_{1}^{-}\)?

\begin{equation} \label{eq:20} P_{1}^{-} = F_{0}P_{0}^{+}F_{0}^{T} + Q_{0} \end{equation}

同样，我们可以推而广之：

\begin{equation} \label{eq:21} P_{k}^{-} = F_{k-1} P_{k-1}^{+} F_{k-1}^{T} + Q_{k-1} \end{equation}

这就是\(P\)的时间更新方程。

接下来，我们要获得\(\hat{x}\)和\(P\)的测量更新方程。给定 \(\hat{x}_{k}^{-}\)我们如何获得\(\hat{x}_{k}^{+}\)呢？我们知道 \(\hat{x}_{k}^{-}\)和\(\hat{x}_{k}^{+}\)是\(x_{k}\)的两个估计，不过 一个考虑了\(y_{k}\)，一个没有考虑\(y_{k}\)。\(\hat{x}\)和\(P\)的更新过 程：

\begin{eqnarray*} K_{k}&=& P_{k-1}H_{k}^{T}(H_{k}P_{k-1}H_{k}^{T} + R_{k})^{-1} \\ &=& P_{k}H_{k}^{T}R_{k}^{-1} \\ \hat{x}_{k} &=& \hat{x}_{k-1} + K_{k}(y_{k} - H_{k}\hat{x}_{k-1}) \\ P_{k} &=& (1-K_{k}H_{k}) P_{k-1} (1-K_{k}H_{k})^{T} + K_{k}R_{k}K_{k}^{T} \\ &=& (P_{k-1}^{-1} + H_{k}^{T}R_{k}^{-1}H_{k})^{-1} \\ &=& (I - K_{k}H_{k})P_{k-1} \end{eqnarray*}

其中\(\hat{x}_{k-1}\)和\(P_{k-1}\)是\(y_{k}\)到达之前的估计和协方差估 计；\(\hat{x}_{k}\)和\(P_{k}\)是\(y_{k}\)到达之后的估计和协方差估计。 我们用\(\hat{x}_{k}^{-}\)和\(P_{k}^{-}\)是\(y_{k}\)到达之前的估计和协 方差。\(\hat{x}_{k}^{+}\)和\(P_{k}^{+}\)是\(y_{k}\)到达之后的估计和协 方差。

我们用\(\hat{x}_{k}^{-}\)代替\(\hat{x}_{k-1}\)，用\(P_{k}^{-}\)代替 \(P_{k-1}\)。用\(\hat{x}_{k}^{+}\)代替\(\hat{x}_{k}\)，用\(P_{k}^{+}\) 代替\(P_{k}\)。

\begin{eqnarray*} K_{k}&=& P_{k}^{-}H_{k}^{T}(H_{k}P_{k}^{-}H_{k}^{T} + R_{k})^{-1} \\ &=& P_{k}^{+}H_{k}^{T}R_{k}^{-1} \\ \hat{x}_{k}^{+} &=& \hat{x}_{k}^{-} + K_{k}(y_{k} - H_{k}\hat{x}_{k}^{-}) \\ P_{k}^{+} &=& (1-K_{k}H_{k}) P_{k}^{-1} (1-K_{k}H_{k})^{T} + K_{k}R_{k}K_{k}^{T} \\ &=& (( P_{k}^{-} )^{-1} + H_{k}^{T}R_{k}^{-1}H_{k})^{-1} \\ &=& (I - K_{k}H_{k})P_{k}^{-} \end{eqnarray*}

上式\(P_{k}^{+}\)的第一个表达式叫做Joseph协方差更新矩阵，是一个叫做 Joseph的人做出的结果。这个结果要比第三个表达式更稳定。第一个表达式保证 \(P_{k}^{+}\)一定是对称的正定矩阵（只要\(P_{k}^{-}\)）是对称正定矩阵。 第三个表达式计算起来比较简单。但是这个表达式不保证\(P_{k}^{+}\)是正定 的，也不保证其是对称的。使用\(K_{k}\)的第二个表达式时，必须使用 \(P_{k}^{+}\)的第二个表达式。这是因为\(K_{k}\)的第二个表达式依赖于 \(P_{k}^{+}\)，所以\(P_{k}^{+}\)必须不能再依赖\(K_{k}\)。

Kalman滤波器还有一个非常重要的特性。我们发现\(P_{k}^{-}\)，\(K_{k}\)和 \(P_{k}^{+}\)的计算都不依赖\(y_{k}\)，只依赖于系统参数 \(F_{k},H_{k},Q_{k}\)和\(R_{k}\)。这意味着\(K_{k}\)可以离散计算，保存 起来。在实际的适时系统中，只有\(\hat{x}_{k}\)才需要适时计算。

在之前我们发现，计算先验估计的时候需要上一次迭代的后验估计；计算后验估 计的时候需要上一次迭代的先验估计。在这一节，我们将会看到，如何分别只用 一个方程计算先验估计或者后验估计。即，在计算先验估计的时候只用到上一次 的先验估计；计算后验估计的时候只用到上一次的后验估计。

我们知道：

\begin{equation} \label{eq:22} \hat{x}_{k+1}^{-} = F_{k}\hat{x}_{k}^{+} + G_{k}u_{k} \end{equation}

把\(\hat{x}_{k}^{+}\)的表达式带入式~(\ref{eq:22})可以得到：

\begin{eqnarray*} \hat{x}_{k+1}^{-}&=&F_{k} ( \hat{x}_{k}^{-} + K_{k} (y_{k} - H_{k} \hat{x}_{k}^{-}) ) + G_{k}u_{k}\\ &=& F_{k}(I - K_{k}H_{k} )\hat{x}_{k}^{-} + F_{k} K_{k}y_{k} + G_{k} u_{k} \end{eqnarray*}

这说明一个先验估计可以根据上一时刻的先验估计直接算出来，不必计算后验估 计。同理，我们可以得到关于估计协方差的先验估计迭代公式。

\begin{equation} \label{eq:23} P_{k+1}^{-} = F_{k}P_{k}^{+}F_{k}^{T} + Q_{k} \end{equation}

我们把\(P_{k}^{+}\)的计算公式带入，则有：

\begin{eqnarray*} P_{k+1}^{-}&=& F_{k} (P_{k}^{-} - K_{k}H_{k}P_{k}^{-} )F_{k}^{T} + Q_{k} \\ &=& F_{k}P_{k}^{-} F_{k}^{T} - F_{k} K_{k}H_{k}P_{k}^{-}F_{k}^{T} + Q_{k} \end{eqnarray*}

这个方程叫做Riccati方程，根据这个方程我们可以不用计算\(P_{k}^{+}\)而直 接从\(P_{k}^{-}\)计算到\(P_{k+1}^{-}\)。

同理，我们也可以推导\(\hat{x}_{k+1}^{+}\)和\(P_{k+1}^{+}\)的一步计算公 式。

\begin{eqnarray} \label{eq:24} \hat{x}_{k}^{+}&=& (I-K_{k}H_{k})(F_{k-1} \hat{x}_{k-1}^{+} + G_{k-1}u_{k-1} ) + K_{k}y_{k} \\ P_{k}^{+} &=& (I-K_{k}H_{k})(F_{k-1}P_{k-1}^{+}F_{k-1}^{T} + Q_{k-1} ) \end{eqnarray}

本文给出了kalman滤波的基本原理以及一些简化计算的方法。kalman滤波于1960 年被发明。截止目前，已经有50多年。期间，出现了诸多变种。有些侧重于性能 提升，有些侧重于计算量的降低。在实际应用中需要酌情考虑，具体问题具体分 析。