详解Kalman Filter

中心思想

现有：

1. 已知上一刻状态，预测下一刻状态的方法，能得到一个“预测值”。（当然这个估计值是有误差的）
2. 某种测量方法，可以测量出系统状态的“测量值”。（当然这个测量值也是有误差的）

我们如何去估计出系统此时真实的状态呢？

答案是需要结合“预测值”和“测量值”。例如我们可以加权求和，但是这个权重要怎么定义，才能准确估计出真实状态呢？这个权重就是Kalman Filter解决的事情。

系统建模

预测方法

\[x_k=F_kx_{k-1}+B_ku_k+w_k
\]

我们假设这个预测方法是线性变换，\(B_ku_k\)是除了状态转移之外的控制量。\(w_k\)是预测误差，假设为高斯分布（即均值为0的多元正态分布），其协方差矩阵为\(Q_k\)。

也就是说，下一刻我们的预测值，有一部分与上一刻状态有关，一部分与外力控制有关（外力控制与上一刻状态无关），还有一部分被噪声所影响。

举个例子：

假设一小车在x轴上向前以速度\(v\)匀速运动，\(x_k\)表示的是k时刻小车在x轴上的坐标。显然我们有$$x_k=x_{k-1}+vt$$，这里速度v和时间t都和上一个状态无关，属于让小车位置变化的“外力”。在这个例子里\(F_kx_{k-1}\)部分就是\(x_{k-1}\)，而\(B_ku_k\)部分就是\(vt\)。

测量方法

\[z_k=H_kx_k+v_k
\]

因为我们不一定有测量仪器能直接测量出系统状态，因此我们假设测量方法也是线性变换。

也就是说，当我们的测量仪器用于测量系统状态\(x_k\)时，它的读数是系统状态加上一定的线性变换\(H_k\)，以及测量噪声\(v_k\)，假设为高斯分布（即均值为0的多元正态分布），其协方差矩阵为\(R_k\)。

举个例子：

我们称体重需要得到以“斤”为单位的数据，这是系统状态，但是我们的称只能读出单位为“kg”的数据（这就是\(z_k\)），那我们就需要做一个单位转换（对应\(H_k\)），此外，由于称不一定准，所以最后称的读数还得加上一点噪声。

所有参数中：\(F_k\)、\(B_k\)、\(u_k\)、\(Q_k\)、\(H_k\)、\(R_k\)都需要已知，要么自己根据公式和经验定义，要么从样本数据里估计一个值。

算法

Kalman Filter将一直维护对系统状态\(x_k\)的最优估计值，以及这个估计值的偏差：

• \(\hat{x}_k\)，系统状态，可以是多维的。
• \(P_k\)，\(\hat{x}_k\)的误差。当\(\hat{x_k}\)是一维时，\(P_k\)是方差；当\(\hat{x_k}\)是多维时，\(P_k\)是协方差\(cov(\hat{x_k})\)，就是\(\hat{x_k}\)里各维两两协方差。

预测阶段

首先，通过系统的预测方法，我们可以得到“预测值”：

\[\bar{x_k}=F_k\hat{x}_{k-1}+B_ku_k
\]

由于误差不知道，且假设其均值为0，所以这里不算误差

那么协方差也可以从上一个状态转移：

\[\bar{P}_k=F_kP_{k-1}F_k^T+Q_k
\]

更新阶段

这个阶段需要结合“预测值”和“测量值”。先把具体的方程摆上来：

• 计算测量值残差（innovation/measurement pre-fit residual）：\(\tilde{y}_k=z_k-H_k\bar{x_k}\)（我的理解是\(H_k\)就是为了把测量值和预测值转换到同一个空间）
• 计算测量值残差的协方差（Innovation (or pre-fit residual) covariance）：\(S_k=H_k\bar{P}_kH_k^T+R_k\)
• 计算卡尔曼增益（Kalman Gain）：\(K_k=\bar{P}_kH_k^TS_k^{-1}\)
• “更新” 最终得到的估计值：\(\hat{x}_k=\hat{x}_{k-1}+K_k\tilde{y}_k\)
• “更新” 最终得到的估计值的协方差：\(P_k=(I-K_kH_k)P_{k-1}\)

总之最后的算法就是，每得到一个“测量值”，就重复一遍预测和更新阶段。

\(\mathbf{K}\)就是Kalman Gain，它衡量了“测量值”和“预测值”之间的权重比例，\(\mathbf{K}\)越大，“测量值”所占权重越大。从公式可以看出:

• \(Q_k\)越大，\(\bar{P}_k\)越大，\(\mathbf{K_k}\)越大。这从物理意义上也是说得通的，当预测值的误差更大的时候，当然应该多信任测量值。
• \(R_k\)越大，\(S_k\)越大，\(\mathbf{K_k}\)越小。也就是说，当“测量值”的误差越大时，该公式将更信任“预测值”。

所有参数中，\(F_k\)、\(B_k\)、\(u_k\)、\(H_k\)基本都需要根据你的问题的建模来决定，而除此之外的\(Q_k\)和\(R_k\)就是两个用于控制预测灵活性的参数，有点类似于指数滑动平均算法的那个\(\alpha\)。

更新阶段原理（试图）详解

结合“预测值”和“测量值”的思想

参考：如何通俗并尽可能详细地解释卡尔曼滤波？ - 肖畅的回答 - 知乎

{{https://img-blog.csdnimg.cn/20200803125815272.png?x-oss-process=image/watermark,type_ZmFuZ3poZW5naGVpdGk,shadow_10,text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L2xlbW9ubWlsbGll,size_16,color_FFFFFF,t_70(uploading...)}}

{{https://img-blog.csdnimg.cn/2020080312584686.png?x-oss-process=image/watermark,type_ZmFuZ3poZW5naGVpdGk,shadow_10,text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L2xlbW9ubWlsbGll,size_16,color_FFFFFF,t_70(uploading...)}}

{{https://img-blog.csdnimg.cn/20200803125913738.png?x-oss-process=image/watermark,type_ZmFuZ3poZW5naGVpdGk,shadow_10,text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L2xlbW9ubWlsbGll,size_16,color_FFFFFF,t_70(uploading...)}}

（结合高斯分布解释）具体公式怎么推导

参考：图说卡尔曼滤波，一份通俗易懂的教程

让我们从一维看起，设方差为\(\sigma^2\)，均值为\(\mu\)，一个标准一维高斯钟形曲线方程如下所示：

\[\mathcal{N}(x,\mu,\sigma)=\frac{1}{\sigma\sqrt{2\pi}}e^{-\frac{(x-\mu)^2}{2\sigma^2}}
\]

那么两条高斯曲线相乘呢？

{{https://img-blog.csdnimg.cn/20200803125956876.png?x-oss-process=image/watermark,type_ZmFuZ3poZW5naGVpdGk,shadow_10,text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L2xlbW9ubWlsbGll,size_16,color_FFFFFF,t_70(uploading...)}}

\[\mathcal{N}(x,\mu_0,\sigma_0)\cdot\mathcal{N}(x,\mu_1,\sigma_1)=\mathcal{N}(x,\mu',\sigma')
\]

把这个式子按照一维方程进行扩展，可得：

\[\mu'=\mu_0+\frac{\sigma_0^2(\mu_1-\mu_0)}{\sigma_0^2+\sigma_1^2}
\]

\[\sigma'^2=\sigma_0^2-\frac{\sigma_0^4}{\sigma_0^2+\sigma_1^2}
\]

如果有些太复杂，我们用k简化一下：

\[\mathbf{k}=\frac{\sigma_0^2}{\sigma_0^2+\sigma_1^2}
\]

\[\mu'=\mu_0+\mathbf{k}(\mu_1-\mu_0)
\]

\[\sigma'^2=\sigma_0^2-\mathbf{k}\sigma_0^2
\]

以上是一维的内容，如果是多维空间，把这个式子转成矩阵格式：

\[\mathbf{K}=\Sigma_0(\Sigma_0+\Sigma_1)^{-1}
\]

\[\mu'=\mu_0+\mathbf{K}(\mu_1-\mu_0)
\]

\[\Sigma'=\Sigma_0-\mathbf{K}\Sigma_0
\]

其中，\(\Sigma\)表示协方差。

代入到Kalman Filter里，我们把“预测分布”\((\mu_0, \Sigma_0)=(H_k\bar{x}_k,H_k\bar{P}_kH_k^T)\)，和“测量分布”\((\mu_1, \Sigma_1)=(z_k,R_k)\)代入到上面的等式里，那么新分布\((\mu',\Sigma')=(H_k\hat{x}_k, H_kP_kH_k^T)\)为：

这里\((\mu_0, \Sigma_0)=(H_k\bar{x}_k,H_k\bar{P}_kH_k^T)\)乘以了系数\(H_k\)是为了把\(x_k\)转换到和\(z_k\)一个坐标系。

\[K=H_k\bar{P}_kH_k^T(H_k\bar{P}_kH_k^T+R_k)^{-1}
\]

\[H_k\hat{x}_k=H_k\bar{x}_k+K(z_k-H_k\bar{x}_k)
\]

\[H_kP_kH_k^T=H_k\bar{P}_kH_k^T-KH_k\bar{P}_kH_k^T
\]

等式两边消掉\(H_k\)并化简后：

\[K_k=\bar{P}_kH_k^T(H_k\bar{P}_kH_k^T+R_k)^{-1}
\]

\[\hat{x}_k=\bar{x}_k+K_k(z_k-H_k\bar{x}_k)
\]

\[P_k=(I-K_kH_k)\bar{P}_k
\]