中心思想

现有：

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

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

系统建模

预测方法

$x_k=F_kx_{k-1}+B_ku_k+w_k$xk​=Fk​xk−1​+Bk​uk​+wk​
我们假设这个预测方法是线性变换，$B_ku_k$Bk​uk​是除了状态转移之外的控制量。$w_k$wk​是预测误差，假设为高斯分布（即均值为0的多元正态分布），其协方差矩阵为$Q_k$Qk​。
也就是说，下一刻我们的预测值，有一部分与上一刻状态有关，一部分与外力控制有关（外力控制与上一刻状态无关），还有一部分被噪声所影响。
举个例子：
假设一小车在x轴上向前以速度$v$v匀速运动，$x_k$xk​表示的是k时刻小车在x轴上的坐标。显然我们有$x_k=x_{k-1}+vt$xk​=xk−1​+vt，这里速度v和时间t都和上一个状态无关，属于让小车位置变化的“外力”。

测量方法

$z_k=H_kx_k+v_k$zk​=Hk​xk​+vk​
因为我们不一定有测量仪器能直接测量出系统状态，因此我们假设测量方法也是线性变换。
也就是说，当我们的测量仪器用于测量系统状态$x_k$xk​时，它的读数是系统状态加上一定的线性变换$H_k$Hk​，以及测量噪声$v_k$vk​，假设为高斯分布（即均值为0的多元正态分布），其协方差矩阵为$R_k$Rk​。
举个例子：
我们称体重需要得到以“斤”为单位的数据，这是系统状态，但是我们的称只能读出单位为“kg”的数据（这就是$z_k$zk​），那我们就需要做一个单位转换（对应$H_k$Hk​），此外，由于称不一定准，所以最后称的读数还得加上一点噪声。

所有参数中：$F_k$Fk​、$B_k$Bk​、$u_k$uk​、$Q_k$Qk​、$H_k$Hk​、$R_k$Rk​都需要已知，要么自己根据公式和经验定义，要么从样本数据里估计一个值。

算法

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

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

预测阶段

首先，通过系统的预测方法，我们可以得到“预测值”：
$\bar{x_k}=F_k\hat{x}_{k-1}+B_ku_k$xk​ˉ​=Fk​x^k−1​+Bk​uk​
由于误差不知道，且假设其均值为0，所以这里不算误差
那么协方差也可以从上一个状态转移：
$\bar{P}_k=F_kP_{k-1}F_k^T+Q_k$Pˉk​=Fk​Pk−1​FkT​+Qk​

更新阶段

这个阶段需要结合“预测值”和“测量值”。结合思想如下：

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

那么具体公式怎么推导呢？结合高斯分布：

参考这里

让我们从一维看起，设方差为$\sigma^2$σ2，均值为$\mu$μ，一个标准一维高斯钟形曲线方程如下所示：
$\mathcal{N}(x,\mu,\sigma)=\frac{1}{\sigma\sqrt{2\pi}}e^{-\frac{(x-\mu)^2}{2\sigma^2}}$N(x,μ,σ)=σ2π​1​e−2σ2(x−μ)2​
那么两条高斯曲线相乘呢？

$\mathcal{N}(x,\mu_0,\sigma_0)\cdot\mathcal{N}(x,\mu_1,\sigma_1)=\mathcal{N}(x,\mu',\sigma')$N(x,μ0​,σ0​)⋅N(x,μ1​,σ1​)=N(x,μ′,σ′)
把这个式子按照一维方程进行扩展，可得：
$\mu'=\mu_0+\frac{\sigma_0^2(\mu_1-\mu_0)}{\sigma_0^2+\sigma_1^2}$μ′=μ0​+σ02​+σ12​σ02​(μ1​−μ0​)​
$\sigma'^2=\sigma_0^2-\frac{\sigma_0^4}{\sigma_0^2+\sigma_1^2}$σ′2=σ02​−σ02​+σ12​σ04​​
如果有些太复杂，我们用k简化一下：
$\mathbf{k}=\frac{\sigma_0^2}{\sigma_0^2+\sigma_1^2}$k=σ02​+σ12​σ02​​
$\mu'=\mu_0+\mathbf{k}(\mu_1-\mu_0)$μ′=μ0​+k(μ1​−μ0​)
$\sigma'^2=\sigma_0^2-\mathbf{k}\sigma_0^2$σ′2=σ02​−kσ02​
以上是一维的内容，如果是多维空间，把这个式子转成矩阵格式：
$\mathbf{K}=\Sigma_0(\Sigma_0+\Sigma_1)^{-1}$K=Σ0​(Σ0​+Σ1​)−1
$\mu'=\mu_0+\mathbf{K}(\mu_1-\mu_0)$μ′=μ0​+K(μ1​−μ0​)
$\Sigma'=\Sigma_0-\mathbf{K}\Sigma_0$Σ′=Σ0​−KΣ0​
其中，$\Sigma$Σ表示协方差。
代入到Kalman Filter里，我们把“预测分布”$(\mu_0, \Sigma_0)=(H_k\bar{x}_k,H_k\bar{P}_kH_k^T)$(μ0​,Σ0​)=(Hk​xˉk​,Hk​Pˉk​HkT​)，和“测量分布”$(\mu_1, \Sigma_1)=(z_k,R_k)$(μ1​,Σ1​)=(zk​,Rk​)代入到上面的等式里，那么新分布$(\mu',\Sigma')=(H_k\hat{x}_k, H_kP_kH_k^T)$(μ′,Σ′)=(Hk​x^k​,Hk​Pk​HkT​)为：

这里$(\mu_0, \Sigma_0)=(H_k\bar{x}_k,H_k\bar{P}_kH_k^T)$(μ0​,Σ0​)=(Hk​xˉk​,Hk​Pˉk​HkT​)乘以了系数$H_k$Hk​是为了把$x_k$xk​转换到和$z_k$zk​一个坐标系。

$K=H_k\bar{P}_kH_k^T(H_k\bar{P}_kH_k^T+R_k)^{-1}$K=Hk​Pˉk​HkT​(Hk​Pˉk​HkT​+Rk​)−1
$H_k\hat{x}_k=H_k\bar{x}_k+K(z_k-H_k\bar{x}_k)$Hk​x^k​=Hk​xˉk​+K(zk​−Hk​xˉk​)
$H_kP_kH_k^T=H_k\bar{P}_kH_k^T-KH_k\bar{P}_kH_k^T$Hk​Pk​HkT​=Hk​Pˉk​HkT​−KHk​Pˉk​HkT​
等式两边消掉$H_k$Hk​并化简后：
$K_k=\bar{P}_kH_k^T(H_k\bar{P}_kH_k^T+R_k)^{-1}$Kk​=Pˉk​HkT​(Hk​Pˉk​HkT​+Rk​)−1
$\hat{x}_k=\bar{x}_k+K_k(z_k-H_k\bar{x}_k)$x^k​=xˉk​+Kk​(zk​−Hk​xˉk​)
$P_k=(I-K_kH_k)\bar{P}_k$Pk​=(I−Kk​Hk​)Pˉk​
$\mathbf{K}$K就是Kalman Gain，它衡量了“测量值”和“预测值”之间的权重比例，$\mathbf{K}$K越大，“测量值”所占权重越大。
从一维结果$\mathbf{k}=\frac{\sigma_0^2}{\sigma_0^2+\sigma_1^2}=\frac{1}{1+\sigma_1^2/\sigma_0^2}$k=σ02​+σ12​σ02​​=1+σ12​/σ02​1​可知，$\sigma_0^2$σ02​越大，$\mathbf{k}$k越大。而当“预测值”的误差越大时，$\sigma_0^2$σ02​越大。也就是说，当“预测值”的误差越大时，该公式将更信任“测量值”。