卡尔曼滤波【Kalman Filter】详解

参考：深入浅出理解卡尔曼滤波【实例、公式、代码和图】

卡尔曼滤波

基本概念

（线性）卡尔曼滤波的应用基于以下三个假设前提：

1. 当前时刻状态只和上一时刻状态有关。
2. 模型和系统均满足线性关系。
3. 引入的噪声符合高斯分布。
   基于上述假设，我们可以得到如下两个表征过程模型和测量模型的公式：
   $$\begin{array}{rl}& x_k=F{x}_{k-1}+B{u}_{k-1}+w_k......①\\ & z_k=H{x}_k+v_k......②\end{array}$$

公式①表示过程模型，公式②表示测量模型，其中：

• $x_k$ 表示 k 时刻的真实值，是待估计的值，例如位置、速度
• $x_{k-1}$ 表示 k-1 时刻的真实值
• $u_{k-1}$ 表示 k-1 时刻的控制输入量，例如加速度等等
• $w_k$ 表示过程噪声，且符合均值为 0，协方差矩阵为 $Q$ 的高斯噪声分布
• $z_k$ 表示 k 时刻的观测值，例如雷达或者 GPS 测量结果，它可能和 $x_k$ 保持相同维度，也可能不同维度。
• $v_k$ 表示测量噪声
• $F、B、H$ 分别表示状态转移矩阵、控制矩阵、观测转移矩阵。

先验估计值

由于过程模型噪声以及真实值未知，不妨忽略噪声，并假设：
$$x_k^{-}=F{\tilde{x}}_{k-1}+B{u}_{k-1}$$
我们使用上一时刻的最优估计值 ${\tilde{x}}_{k-1}$ 来替代真实值，并将 $x_k^{-}$ 称为当前时刻通过过程模型得到的预测值，也叫做先验估计值。

后验（最优）估计值

显然，这是一个误差较大的预估值，我们使用观测值来进行修正。
$${\tilde{x}}_k=x_k^{-}+K(z_k-H{x}_k^{-})......③$$
$K$ 表示卡尔曼增益，当前为未知量，${\tilde{x}}_k$ 为最优估计值，由于和当前时刻的观测量有关系，也称为后验估计值。因此我们希望求解一个合适的 $K$，使得最优估计值最接近真实值。

目标函数的建立与转化

设 $e_k=x_k-{\tilde{x}}_k$，表示最优值和真实值的误差。求解最优目标函数 ${\min }_K|e_k|$。
将③带入公式②，并且设 $e_k^{-}=x_k-x_k^{-}$，$e_k^{-}$ 表示预测值和真实值的误差，得到：
$$e_k=(I-KH)e_k^{-}-K{v}_k......④$$
直接通过随机变量的关系式来求解最优目标函数显然不可行，我们通过表征随机变量的特征值来进行求解，最简单的特征值就是数学期望。不妨设：

• ⭐ $P_k=E[e_k{e}_k^t]$，表示的是真实值和最优值的后验误差协方差矩阵，更新后作为下一次迭代的输入。
• ⭐ $P_k^{-}=E[e_k^{-}{e}_k^{-t}]$，表示的是真实值和预测值的先验误差协方差矩阵，通过使它的迹最小（方差和最小）使得误差最小，以此求出卡尔曼增益。

协方差矩阵是什么：

方差是用来度量单个随机变量的离散程度，而协方差则一般用来刻画两个随机变量的相似程度，协方差的计算公式被定义为：
$$\sigma \left(x,y\right)=\frac{1}{n-1}\sum _{i=1}^n\left(x_i-\bar{x}\right)\left(y_i-\bar{y}\right)$$
方差可视作随机变量关于其自身的协方差。
协方差矩阵为：
$$\Sigma =\left[\begin{array}{ccc}\sigma (x_1,x_1)& \cdots & \sigma \left(x_1,x_d\right)\\ ⋮& \ddots & ⋮\\ \sigma \left(x_d,x_1\right)& \cdots & \sigma (x_d,x_d)\end{array}\right]\in {\mathbb{R}}^{d\times d}$$
其中，对角线上的元素为各个随机变量的方差，非对角线上的元素为两两随机变量之间的协方差，根据协方差的定义，我们可以认定：矩阵 Σ 为对称矩阵(symmetric matrix)，其大小为 $d$ × $d$ 。

可以得到
$$\begin{array}{rl}{P}_k& =(I-KH)P_k^{-}(I-KH{)}^t+KE[v_k{v}_k^t]K^t\\ & =(I-KH)P_k^{-}(I-H^t{K}^t)+KR{K}^t\\ & =P_k^{-}-KH{P}_k^{-}-P_k^{-}{H}^t{K}^t+K(H{P}_k^{-}{H}^t+R)K^t\end{array}$$
至此，我们将随机变量的最优化问题转化成为了纯数量问题。

但还不够，很多文章在此直接对 $P_k$ 取迹然后对卡尔曼增益 $K$ 求导，然后获得使 $tr(P_k)$ 最小的 $K$ 值。$tr(P_k)$ 表示的是 $P_k$ 主对角线之和，恰好为所有待估计量的方差之和，根据最小二乘法求解最小化 MSE 问题，存在最优解 $K$，使得 ${\min }_{K}tr(P_k)$ 最小。

卡尔曼增益求解

我们求解最优卡尔曼增益$K$，有
$$\begin{array}{rl}\frac{\partial tr(P_k)}{\partial K}& =\frac{\partial tr(P_k^{-})}{\partial K}-\frac{\partial tr(KH{P}_k^{-})}{\partial K}-\frac{\partial tr(P_k^{-}{H}^t{K}^t)}{\partial K}+\frac{\partial tr(K(H{P}_k^{-}{H}^t+R)K^t)}{\partial K}\\ & =0-(H{P}_k^{-}{)}^t-(H{P}_k^{-t}{)}^t+K[(H{P}_k^{-}{H}^t+R)+(H{P}_k^{-}{H}^t+R{)}^t]\\ & =-2P_k^{-}{H}^t+2K(H{P}_k^{-}{H}^t+R)\\ & =0\end{array}$$
总之，得到：
$$K=P_k^{-}{H}^t(H{P}_k^{-}{H}^t+R{)}^{-1}$$

协方差矩阵化简

$$P_k=(I-KH)P_k^{-}$$
$$P_k^{-}=F{P}_{k-1}{F}^t+Q$$

总结

状态方程

$$\begin{array}{rl}& x_k=F{x}_{k-1}+B{u}_{k-1}+w_k......①\\ & z_k=H{x}_k+v_k......②\end{array}$$

预测

先验估计：根据上一次最优估计的结果，预测当前时刻的估计值，由于缺失过程噪声，所以是不完整的，先验的。
$$x_k^{-}=F{\tilde{x}}_{k-1}+B{u}_{k-1}$$
先验估计协方差：
$$P_k^{-}=F{P}_{k-1}{F}^t+Q$$

更新

kalman 增益：先验估计值与实际值存在误差，该误差也假设符合正太分布，为了使误差最小，即求误差方差最小，通过对Ｋ求导等于０，求出Ｋ值，此时误差最小。根据调整测量误差Ｒ，可以调整增益是更相信观测还是更相信预测。
$$K=P_k^{-}{H}^t(H{P}_k^{-}{H}^t+R{)}^{-1}$$
后验估计：
$${\tilde{x}}_k=x_k^{-}+K(z_k-H{x}_k^{-})$$
后验估计协方差：
$$P_k=(I-KH)P_k^{-}$$