Kalman Filter

参考：
Modern Control Systems, 12th, by Richard C. Dorf, Robert H. Bishop, Chapter 3 State Variable Models
https://en.wikipedia.org/wiki/State-space_representation

Kalman Filter 假设 \(k\) 时刻系统的真实状态从 \(k-1\) 时刻演化而来，满足下面离散随机差分方程：
\[\boldsymbol{x_k} = \mathbf{F_k}\boldsymbol{x_{k-1}} + \mathbf{B_k}\boldsymbol{u_{k}} + \boldsymbol{w_{k}}\]
系统的测量方程为：
\[\boldsymbol{z_k} = \mathbf{H_k}\boldsymbol{x_{k}} + \boldsymbol{v_k}\]

说明 1：系统离散随机差分方程中， \(\boldsymbol{x_k}\) 是 \(k\) 时刻系统的状态（如速度等）， \(\boldsymbol{u_{k}}\) 是 \(k\) 时刻对系统的控制量（如方向盘转角等）。 \(\mathbf{F_k}\) 和 \(\mathbf{B_k}\) 是系统参数， \(\mathbf{F_k}\) 称为状态转移矩阵， \(\mathbf{B_k}\) 称为输入输出矩阵。 \(\boldsymbol{w_{k}}\) 是过程噪声，假设其服从正态分布 \(p(\boldsymbol{w_k}) \sim N(0, \mathbf{Q})\) 。
说明 2：系统测量方程中， \(\boldsymbol{z_k}\) 是 \(k\) 时刻系统状态的测量值（用传感器等直接或间接测量系统的状态）， \(\mathbf{H_k}\) 称为测量矩阵（如果系统的所有状态都可以直接测量得到，则测量矩阵就是单位矩阵）。 \(\boldsymbol{v_{k}}\) 是测试噪声，假设其服从正态分布 \(p(\boldsymbol{v_k}) \sim N(0, \mathbf{R})\) 。

卡尔曼滤波建立在隐马尔可夫模型（hidden Markov model）上。其基本动态系统可以用一个马尔可夫链表示，该马尔可夫链建立在一个被高斯噪声（即正态分布的噪声）干扰的线性算子上的。

说明 1：上图中圆圈代表向量，方块代表矩阵，星号代表高斯噪声，其协方差矩阵在右下方标出。图片摘自https://zh.wikipedia.org/wiki/%E5%8D%A1%E5%B0%94%E6%9B%BC%E6%BB%A4%E6%B3%A2
说明 2：实际上，很多真实世界的动态系统都并不确切地符合这个模型；但是由于卡尔曼滤波器被设计在有噪声的情况下工作，一个近似的符合已经可以使这个滤波器非常有用了。

Kalman 滤波器的应用非常广泛，比如雷达系统，机器人导航等等。近年来也应用于计算机图像处理领域，例如人脸识别，图像分割，图像边缘检测等等。

参考：
https://en.wikipedia.org/wiki/Kalman_filter#Applications

卡尔曼滤波包括两个阶段：预测和更新（即校正）；在估计阶段，滤波器应用上一状态的估计做出对当前状态的估计。在更新阶段，滤波器利用在当前状态的观测值优化预测阶段的预测值，以获的一个更精确的当前状态的估计。

离散卡尔曼滤波器算法 5 个公式如下（仅结出结论，不推导）：

\[\begin{array}{cc} \text{离散卡尔曼滤波器时间更新方程（预测阶段的 2 个公式）} & \text{离散卡尔曼滤波器状态更新方程（校正阶段的 3 个公式）} \\ \hline \\ \boldsymbol{\hat{x}_{k}^{-}} = \mathbf{F} \boldsymbol{\hat{x}_{k-1}} + \mathbf{B} \boldsymbol{\hat{u}_{k-1}} & \mathbf{K}_{k} = \mathbf{P}^{-}_{k} \mathbf{H}^{\mathsf{T}} (\mathbf{H} \mathbf{P}^{-}_{k} \mathbf{H}^{\mathsf{T}} + \mathbf{R})^{-1} \\ \mathbf{P}^{-}_{k} = \mathbf{F} \mathbf{P}_{k-1} \mathbf{F}^{\mathsf{T}} + \mathbf{Q} & \boldsymbol{\hat{x}_{k}} = \boldsymbol{\hat{x}_{k}^{-}} + \mathbf{K}_{k} (\boldsymbol{z_k} - \mathbf{H} \boldsymbol{\hat{x}_{k}^{-}}) \\ & \mathbf{P}_{k} = (\mathbf{I} - \mathbf{K}_{k} \mathbf{H}) \mathbf{P}^{-}_{k} \\ \end{array}\]

说明 1：上面式子中符号 \({}^-\) 表示“先验”， \(\hat{}\) 表示“估计”。
说明 2： \(\mathbf{F}, \mathbf{B}, \mathbf{Q}, \mathbf{H}, \mathbf{R}\) 为系统的参数，系统模型建立好后是已知的。
说明 3： \(\mathbf{K}\) 称为卡尔曼增益(Kalman Gain)。 \(\mathbf{P}\) 是误差协方差。编程实现时可以认为是“中间变量”。
说明 4： \(\boldsymbol{\hat{x}_{k}} = \boldsymbol{\hat{x}_{k}^{-}} + \mathbf{K}_{k} (\boldsymbol{z_k} - \mathbf{H} \boldsymbol{\hat{x}_{k}^{-}})\) 中 \(\boldsymbol{\hat{x}_{k}}\) 是想要得到的估计值， \(\boldsymbol{\hat{x}_{k}^{-}}\) 是通过系统状态方程得到的理论预测值， \(\boldsymbol{z_k}\) 是系统的测量值。
说明 5：尽管没有介绍这些公式的推导，由这 5 个公式，我们可以直接编程实现卡尔曼滤波器了。

由牛顿第二定律，我们知道其加速度为 \(\frac{f_t}{m}\) ，这可以看作是系统的控制信号，即有： \(u_t = \frac{f_t}{m}\)
由力学知识有：
\[\begin{gathered} s(t) = s(t-1) + v(t-1) \Delta t + \frac{1}{2} \frac{f_t}{m} (\Delta t)^2 \\ v(t) = v(t-1) + \frac{f_t}{m} \Delta t \end{gathered}\]

如果我们把位置和速度看作是系统的“状态变量”，上面两式又可写为：
\[\left( \begin{array}{c} s_t \\ v_t \end{array} \right) = \left( \begin{array}{cc} 1 & \Delta t \\ 0 & 1 \end{array} \right) \left( \begin{array}{c} s_{t-1} \\ v_{t-1} \end{array} \right) + \left( \begin{array}{c} \frac{(\Delta t)^2}{2} \\ \Delta t \end{array} \right)^2 \frac{f_t}{m}\]

上式就是系统的状态方程。显然，系统的参数 \(\mathbf{F_t}, \mathbf{B_t}\) 分别为：
\[\mathbf{F_t} = \left( \begin{array}{cc} 1 & \Delta t \\ 0 & 1 \end{array} \right), \mathbf{B_t} = \left( \begin{array}{c} \frac{(\Delta t)^2}{2} \\ \Delta t \end{array} \right)^2\]

系统状态方程为：
\[\boldsymbol{x_k} = \mathbf{F_k}\boldsymbol{x_{k-1}} + \mathbf{B_k}\boldsymbol{u_{k}} + \boldsymbol{w_{k}}\]
系统的测量方程为：
\[\boldsymbol{z_k} = \mathbf{H_k}\boldsymbol{x_{k}} + \boldsymbol{v_k}\]

由系统的状态方程可以得到一个理论预测值（前面例子用牛顿第二定律得到），而通过实际测量（如速度传感器等）可以得到一个测量值。这两个值都存在噪声（误差）。
怎么通过这两个值来估计实际值呢？Kalman 滤波器就是来完成这个工作的。

Kalman 滤波器算法的第 4 个公式告诉我们可以这样得到一个比较好的估计值： \(\boldsymbol{\hat{x}_{k}} = \boldsymbol{\hat{x}_{k}^{-}} + \mathbf{K}_{k} (\boldsymbol{z_k} - \mathbf{H} \boldsymbol{\hat{x}_{k}^{-}})\) 中 \(\boldsymbol{\hat{x}_{k}}\) 是想要得到的估计值， \(\boldsymbol{\hat{x}_{k}^{-}}\) 是通过系统状态方程得到的理论预测值， \(\boldsymbol{z_k}\) 是系统的测量值。

式子 \(\boldsymbol{\hat{x}_{k}} = \boldsymbol{\hat{x}_{k}^{-}} + \mathbf{K}_{k} (\boldsymbol{z_k} - \mathbf{H} \boldsymbol{\hat{x}_{k}^{-}})\) 可直观地理解为 卡尔曼增益 \(\mathbf{K}_{k}\) 决定了理论预测值和实际测量值在估计值中的比重 。
如果状态变量能直接通过传感器测量，则测量矩阵就是单位矩阵，即 \(\mathbf{H} = \mathbf{I}\) 。

参考：
https://www.cl.cam.ac.uk/~rmf25/papers/Understanding%20the%20Basis%20of%20the%20Kalman%20Filter.pdf

在前面介绍的例子中，Kalman Filter 的作用是“去除测量数据中的噪声”，这在随机过程理论中称为滤波问题(Filtering problem)。

参考：https://en.wikipedia.org/wiki/Filtering_problem_(stochastic_processes)

Kalman Filter 算法中，卡尔曼增益决定了系统状态变量“测量值”在系统状态变量“估计值”中的所占比重。

说明：上面式子对原始公式作了很多简化，它假设测量矩阵为单位矩阵，假设先验值 \(\boldsymbol{\hat{x}_{k}^{-}}\) 等于上一次的估计值 \(\boldsymbol{\hat{x}_{k-1}}\) 。

参考：Kalman Filter For Dummies: http://bilgin.esme.org/BitsAndBytes/KalmanFilterforDummies

系统的状态变量只有一个（即电压），所以状态空间模型中的各个矩阵都简化成了单个数。由于假设电压是不变的，所以 \(\mathbf{F}=1\) ；又由于系统没有控制量，所以 \(\boldsymbol{u_{k}}=0\) 。系统状态变量电压是直接测量得到的，所以 \(\mathbf{H}=1\) 。

系统的状态方程为：
\[\boldsymbol{x_k} = \mathbf{F_k}\boldsymbol{x_{k-1}} + \mathbf{B_k}\boldsymbol{u_{k}} + \boldsymbol{w_{k}} = \boldsymbol{x_{k-1}} + \boldsymbol{w_{k}}\]
系统的测量方程为：
\[\boldsymbol{z_k} = \mathbf{H_k}\boldsymbol{x_{k}} + \boldsymbol{v_k} = \boldsymbol{x_{k}} + \boldsymbol{v_k}\]

由于假设电压恒定不变，可假设 \(Q=0\) ；题中已知测量噪声 \(R=0.1 V\) 。

利用前面的状态空间模型，代入系统的参数，Kalman 滤波器算法的 5 个公式如下：
\[\begin{array}{cc} \text{Time Update (prediction)} & \text{Measurement Update (correction)} \\ \hline \\ \hat{x}_{k}^{-} = \hat{x}_{k-1} & K_k = \frac{P^{-}_{k}}{P^{-}_{k} + 0.1} \\ P_{k}^{-} = P_{k-1} & \hat{x}_{k} = \hat{x}_{k}^{-} + K_k (z_k - \hat{x}_{k}^{-}) \\ & P_{k} = (1 - K_k) P^{-}_{k} \\ \end{array}\]

开始迭代前，需要给 \(x\) 和 \(p\) 设定一个初值，这里假设它们的初值分别为 0 和 1。

下面将演示电压估计 \(\hat{x}_k\) 的迭代求解过程。

通过上面迭代求得的电压估计 \(\hat{x}_k\) 如下图所示：