理论讲解

卡尔曼滤波属于线性滤波器，它可以在多元不确定信息下通过融合多个信息源来得到一种最优的状态估计。卡尔曼滤波在连续变化的线性系统中表现是非常出色的，因为它考虑了系统过程中存在的一些干扰，比如模型预测干扰$Q$Q和测量过程干扰$R$R，因此，即使系统中伴随着一些干扰，卡尔曼滤波器也可以比较准确的计算出实际的状态，并且可以对系统未来的运动状态做出合理的预测。卡尔曼滤波器的前提条件是系统是线性高斯系统，线性高斯系统需要从两个方面的理解：

1. 线性：运动方程、观测方程是线性的

2. 高斯：系统噪声服从零均值的高斯分布

综合来说也就是说高斯分布的噪声在经过状态转移之后仍然保持高斯分布。如果不满足这个线性这个前提条件，就需要使用扩展卡尔曼滤波（extended kalman filter ，EKF）和无迹（损）卡尔曼滤波了（Unscented Kalman Filter ，UKF）来代替了。另外卡尔曼滤波器也假设状态是符合正态分布的。

如下图2.15所示，卡尔曼滤波器的信息源一方面来自传感器检测出的车辆状态信息 $\hat{z_k}$zk​^​，另一方面来自数学模型预测出的车辆状态信息 $\hat{x_k}$xk​^​，但这两方面信息源都只是间接的预测，并且伴随着一些不确定和不准确性，如果使用上所有可用的信息源，那么就能得到一个比单一的任何信息源估计更好的结果$\hat{x_k}'$xk​^​′。卡尔曼滤波通过两个高斯分布相乘，就可以把多个不确定信息源融合在一起。传感器检测的车辆状态信息$\hat{z_k}$zk​^​在某种情况下符合一个高斯正太分布，预测模型计算出的车辆状态信息也$\hat{x_k}$xk​^​符合一个高斯正太分布，而后将这两个独立的高斯分布相乘就会得到一个新的高斯分布$\hat{x_k}'$xk​^​′。新的高斯分布就是原来两个高斯分布重叠的区域，其均值就代表着最优的状态估计。

卡尔曼滤波的实质是预测的车辆状态和测量的车辆状态两个高斯分布相乘的到新的高斯分布，该过程最后可以凝练成五个核心公式：

预测公式：

$\hat x_k=F_k \hat x_{k-1} + B_k \vec u_k\\ P_k=F_kP_{k-1}F^T_k+Q_k$x^k​=Fk​x^k−1​+Bk​uk​Pk​=Fk​Pk−1​FkT​+Qk​
更新公式：

$K' = P_kH^T_k(H_kP_kH^T_k+R_k)^{-1}\\ \hat x'_k = \hat x_k+K'(\vec {z_k}-H_k\hat x_k)\\ P'_k=P_k-K'H_kP_k$K′=Pk​HkT​(Hk​Pk​HkT​+Rk​)−1x^k′​=x^k​+K′(zk​​−Hk​x^k​)Pk′​=Pk​−K′Hk​Pk​
其中 $\hat{x_k}$xk​^​为$k$k时刻的状态向量，$F_k$Fk​为状态转移矩阵，表示将$k-1$k−1时刻的状态向量转移至t时刻的状态向量， $B_k$Bk​是输入控制矩阵，代表着将控制向量 $\hat{u_k}$uk​^​映射到状态向量上，统一控制向量 $\hat{u_k}$uk​^​和状态向量之间 $\hat{x_k}$xk​^​的关系，$\hat{u_k}$uk​^​代表着控制向量，如加速度，角加速度等，$P_k$Pk​为状态向量的协方差矩阵，代表着状态向量每个元素之间的关系， $Q_k$Qk​表示预测状态的高斯噪声的协方差阵，它用来衡量模型的准确度，模型越准确其值越小， $\vec {z_k}$zk​​为传感器测量值的状态向量，也就是传感器的测量结果，$H_k$Hk​ 为转换矩阵，他将状态向量 $\hat{x_k}$xk​^​映射到测量值所在的向量空间$\vec {z_k}$zk​​，$R_k$Rk​为测量值的高斯噪声的协方差阵，代表着传感器测量的误差。

公式推导

那卡尔曼滤波公式的五个公式是如何推导的呢？

用均值和方差描述物体状态

怎么去描述某一时刻物体的状态呢? 状态$\hat{x_k}$xk​^​可以包含多个变量，代表任何你想表示的信息， 我们用均值和方差来表示某一时刻的状态，为啥是这两个呢? 均值就是我们定义要知道的一些变量状态，例如位置，速度，体积，温度等等。它表示随机分布的中心，即最可能的状态。方差表示一种偏离均值的程度，他可以告诉我们更多信息，利用其中一个变量值告诉我们其它变量可能的值。例如，位置和速度的关系。我们基于旧的位置来估计新位置。如果速度过高，我们可能已经移动很远了。如果缓慢移动，则距离不会很远。跟踪这种关系是非常重要的，因为它带我们更多的信息：其中一个测量值告诉了我们其它变量可能的值，这就是卡尔曼滤波的目的，尽可能地在包含不确定性的测量数据中提取更多信息！这种相关性用协方差矩阵来表示。矩阵中的每个元素 $\Sigma_{ij}$Σij​ 表示第 $i$i个和第 $j$j个状态变量之间的相关度。（协方差矩阵是一个对称矩阵，这意味着可以任意交换 $i$i 和$j$j）。协方差矩阵通常用“$\Sigma$Σ”来表示，其中的元素则表示为“ $\Sigma_{ij}$Σij​ ”。

所以在时刻 $k$k 物体的状态为 ：均值 $\hat{x_k}$xk​^​和协方差矩阵$P_k$Pk​ 。
$\hat{x_k}=\left[ \begin{matrix} position \\ velocity \\ \end{matrix} \right] \quad\quad\quad P_k=\left[ \begin{matrix} \Sigma_{pp}&\Sigma_{pv} \\ \Sigma_{vp}&\Sigma_{vv} \\ \end{matrix} \right]$xk​^​=[positionvelocity​]Pk​=[Σpp​Σvp​​Σpv​Σvv​​]

状态转移矩阵$F$F表示预测

接下来，我们需要根据当前状态（$k-1$k−1 时刻）来预测下一状态（$k$k 时刻），怎么去预测呢?

我们可以用矩阵 $F_k$Fk​来表示这个预测过程。这个 $F_k$Fk​就是根据运动学或者动力学等物理公式得到的内在关系或者规律。记住，我们并不知道对下一状态的所有预测中哪个是“真实”的，但我们的预测函数并不在乎。它对所有的可能性进行预测，并给出新的高斯分布。

它将我们原始估计中的每个点都移动到了一个新的预测位置，如果原始估计是正确的话，这个新的预测位置就是系统下一步会移动到的位置。那我们又如何用矩阵来预测下一个时刻的位置和速度呢？下面用一个基本的运动学公式来表示：矩阵表示
$p_k=p_{k-1}+\Delta tv_{k-1}\\ v_k=\quad\quad \quad\quad v_{k-1}$pk​=pk−1​+Δtvk−1​vk​=