• 如何使用卡尔曼滤波器（Kalman Filtering）预测物体的运动轨迹？
• 卡尔曼滤波器和目标跟踪
• 卡尔曼滤波算法原理

文章的大部分内容都是根据参考资料收集的，旨在更方便学习记忆搜索，而不是其他用途。

• 卡尔曼滤波（Kalman filtering, KF）利用线性系统状态方程，通过系统输入输出观测数据，对系统状态进行最优估计。
• 卡尔曼滤波器的一个典型例子是从一组有限的坐标位置和速度中预测物体的位置，包括噪声观察序列。它可以在许多工程应用程序（雷达、计算机视觉）中找到。同时，卡尔曼滤波器也是控制理论和控制系统工程中的一个重要话题。
• 解决普通问题KF只能处理线性数据的问题，从而基于非线性数据KF推广:扩展卡尔曼滤波器（EKF）和无迹卡尔曼滤波（UKF）。KF系统方程和测量方程主要用于线性场景，EKF适用于轻度非线性场景，UKF在强非线性条件下表现更好。

• 先验概率： 从以往的经验和分析中获得的概率。也可视为无条件概率。

  其中，先验信息一般来自经验和历史资料。比如林丹和一个选手对决，解释一般会根据林丹之前的比赛结果对比赛的胜负做出大致的判断。另一个例子是，工厂每天对产品进行质量检验，以评估产品的不合格率θ，一段时间后，会积累大量的历史资料。这些历史资料是先验知识。有了这些先验知识，就有了决定一个产品是否需要每天质量检验的依据。如果之前的历史资料显示某个产品的不合格率只有0.01%可视为值得信赖的产品或免检产品，每月只抽检一两次，从而节省量的人力物力。

• 后验概率：后验概率是基于新信息，修改原先验概率后更接近实际情况的概率估计。直观地说，后验概率是条件概率。

这部分主要来自知乎文章。

或者卡尔曼滤波过程在下面的文献材料中通常有意义:

以上两种形式只是符号不同。

卡尔曼滤波模型假设k时刻的真实状态是从（k ? 1)符合以下公式的时，符合以下公式:

• 状态估计方程 x k = F k x k ? 1 B k u k w k x_{k}=F_{k} x_{k-1} B_{k} u_{k} w_{k} xk=Fkxk?1+Bk​uk​+wk​ 其中： F k F_{k} Fk​为状态变换矩阵，即状态转移矩阵。 B k B_{k} Bk​是作用在控制器向量 u k u_{k} uk​上的输入-控制矩阵。 B k u k B_{k} u_{k} Bk​uk​这一项是指我们在追踪一个物体的状态的时候把它内部的控制也考虑进去了，一般运动学中没有这项，因为对于检测的目标是无法测量其内部的控制量，所以简化为0。 w k ∼ N ( 0 , Q k ) w_{k} \sim N\left(0, Q_{k}\right) wk​∼N(0,Qk​)是过程噪声，均值为0， Q k Q_{k} Qk​为过程噪声协方差矩阵。

• 状态估计转移方程 z k = H k x k + v k z_{k}=H_{k} x_{k}+v_{k} zk​=Hk​xk​+vk​ 表示k时刻真实状态 x k x_{k} xk​的一个测量，其中： H k H_{k} Hk​为测量矩阵 (观测矩阵)，它把真实状态空间映射成观测空间， v k v_{k} vk​是测量噪声（观测噪声）。 初始状态以及每一时刻的噪声（过程噪声与测量噪声）都认为是互相独立的，即 x 0 , w 1 , … , w k , v 1 … v k x_{0}, w_{1}, \ldots, w_{k}, v 1 \ldots v_{k} x0​,w1​,…,wk​,v1…vk​是互相独立的。

第一个公式为状态估计方程，用来预测下一步状态（故 F k F_{k} Fk​也叫预测矩阵）。其中： x ^ k ∣ k − 1 \hat{x}_{k \mid k-1} x^k∣k−1​为k-1时刻对k时刻的状态预测。 x ^ k − 1 ∣ k − 1 \hat{x}_{k-1 \mid k-1} x^k−1∣k−1​在时刻k-1的状态估计。

第二个公式为预测估计协方差方程，计算先验概率。其中： P k − 1 ∣ k − 1 P_{k-1 \mid k-1} Pk−1∣k−1​为k-1时刻的后验估计误差协方差矩阵，度量估计值的精确程度。 P k ∣ k − 1 P_{k \mid k-1} Pk∣k−1​为k-1时刻到k时刻的估计误差协方差矩阵。 Q k Q_{k} Qk​为过程噪声协方差矩阵，越大说明越不相信预测。

• 第一个公式为卡尔曼增益方程。 K k K_k Kk​为最优卡尔曼增益。 R k R_k Rk​为测量噪声协方差矩阵，越大说明越不相信观测。实际工程中测量噪声由厂商提供，也可以自己调参，也可以自适应（AKF）。

• 第二个公式在更新状态估计方程，也就是最终的滤波状态结果。 可以使用测量残差来简化表达公式

  y ^ = z k − H k x ^ k ∣ k − 1 S k = cov ⁡ ( y ^ ) = H k P k ∣ k − 1 H k T + R k \hat{y}=z_{k}-H_{k} \hat{x}_{k \mid k-1}\\ S_{k}=\operatorname{cov}(\hat{y})=H_{k} P_{k \mid k-1} H_{k}^{T}+R_{k} y^​=zk​−Hk​x^k∣k−1​Sk​=cov(y^​)=Hk​Pk∣k−1​HkT​+Rk​

  所以第1，2个公式可以简化为：

  K k = P k ∣ k − 1 H k T S k − 1 x ^ k ∣ k = x ^ k ∣ k − 1 + K k y ^ K_{k}=P_{k \mid k-1} H_{k}^{T} S_{k}^{-1}\\ \hat{x}_{k \mid k}=\hat{x}_{k \mid k-1}+K_{k} \hat{y} Kk​=Pk∣k−1​HkT​Sk−1​x^k∣k​=x^k∣k−1​+Kk​y^​

• 第三个公式在更新估计协方差方程，计算得到后验概率。

所以简化后的更新部分的公式有5个：

以上部分已经能够知道卡尔曼滤波在干嘛了，但要想知其然且知其所以然，那还得进行公式推导，这样也更有利于记忆。

已知: Q k = cov ⁡ ( w k ) = E ( w k w k T ) R k = cov ⁡ ( v k ) = E ( v k v k T ) P k ∣ k − 1 = cov ⁡ ( x k − x ^ k ∣ k − 1 ) = E ( ( x k − x ^ k ∣ k − 1 ) ( x k − x ^ k ∣ k − 1 ) T ) P k ∣ k = cov ⁡ ( x k − x ^ k ∣ k ) = E ( ( x k − x ^ k ∣ k ) ( x k − x ^ k ∣ k ) T ) S k = cov ⁡ ( y ^ ) = cov ⁡ ( z k − H k x ^ k ∣ k − 1 ) (1) \begin{aligned} \tag{1} &Q_{k}=\operatorname{cov}\left(w_{k}\right)=E\left(w_{k} w_{k}^{T}\right) \quad \\ &R_{k}=\operatorname{cov}\left(v_{k}\right)=E\left(v_{k} v_{k}^{T}\right) \\ &P_{k \mid k-1}=\operatorname{cov}\left(x_{k}-\hat{x}_{k \mid k-1}\right)=E\left(\left(x_{k}-\hat{x}_{k \mid k-1}\right)\left(x_{k}-\hat{x}_{k \mid k-1}\right)^{T}\right) \\ &P_{k \mid k}=\operatorname{cov}\left(x_{k}-\hat{x}_{k \mid k}\right)=E\left(\left(x_{k}-\hat{x}_{k \mid k}\right)\left(x_{k}-\hat{x}_{k \mid k}\right)^{T}\right) \\ &S_{k}=\operatorname{cov}(\hat{y})=\operatorname{cov}\left(z_{k}-H_{k} \hat{x}_{k \mid k-1}\right) \end{aligned} ​Qk​=cov(wk​)=E(wk​wkT​)Rk​=cov(v