卡尔曼滤波视频学习记录

资源来源：Michel van Biezen SPECIAL TOPICS THE KALMAN FILTER

官网：http://www.ilectureonline.com/lectures/subject/SPECIAL%20TOPICS/26/190

哔哩哔哩：https://www.bilibili.com/video/av36187842/?p=1

该视频教程简单、易懂，是入门级很不错的教程！

附带一个可以免费翻墙的浏览器 链接：https://pan.baidu.com/s/1Uip__jFc0CFZIpAfU2tGMg 提取码：fu1u

Lecture 1: What is A Kalman Filter?

Definition: it is an iterative mathematical process, use a set of equations and consecutive data inputs to quickly estimate the true value, position, velocity, etc of the object being measured.When the measure values contain unpredicted or random error, uncertainty or variation.

解释：卡尔曼滤波是一种对感兴趣的信号进行迭代更新的过程，它利用几个公式结合当前测量值（一般具有测量误差、随机误差等特性）的输入，对感兴趣的信号的真实值进行快速、最优估计。

例子：

上图是利用KF对温度计测量的温度进行滤波，温度计由于自身的误差原因，得到的温度并不是真实值，会出现上下波动，如图中的离散点。真实值理论上上一条水平的直线，此处利用KF来对温度测量值进行滤波，可以快速地逼近真实值这条曲线。

通常KF的应用主要是融合多个测量值，如在定位方面的有GPS、IMU、雷达等手段获取定位结果，这几种手段各有优缺点，此时采用KF将其进行融合处理，使其优势互补，增强系统的测量精度以及鲁棒性。

Lecture 2: Flowchart of A Simple Example

上图是卡尔曼滤波的简易版框图，KF的主要流程为(1) Calculate Gain, (2) Calculate Current Estimate, (3) Calculate New Estimate Error. 每一次迭代都是进行上述三个流程，从而获得当前的最优估计（Update Estimate）。

（1）计算误差增益：对预测误差和测量数据误差进行一种权衡。简单来说，增益代表的就是更加相信那个，谁的误差小，就多相信些，加权的时候比例更大。

（2）更新当前估计：利用预测值和测量值，结合第一步得到的增益来估计当前的值。

（3）更新预测误差：利用第二步的更新值以及第一步的增益值计算下一步的预测误差。

备注：Original Error Estimate初值一般取大一些，这样刚开始的时候会比较相信测量值，保证预测不会太过偏离真实值，后面更新迭代过程中会随着时间逐渐改变。

Lecture 3: The Kalman Gain: A Closer Look

如图所示为卡尔曼增益KG定义以及在迭代公式中的作用。

KG定义为预测误差与预测+测量误差的比例，范围在0-1之间波动。

如图中的KG趋势图所示，当KG为1时，表明测量误差很小，比较精确，此时我们选择更加相信测量值，即在更新公式后面部分比较重要，作为当前时刻的更新增量。反之，KG为0时，表示预测误差很小，预测值比较稳定，此时更加相信预测值。

备注：理论上KG会来越来小，因为KF会快速收敛，接近真实值。

Lecture 4: The 3 Calculations of The Kalman Filter

如图为迭代更新过程中的三个阶段对应的三个公式。

第一个公式为KG计算方法，表示测量和预测误差的比例，KG越大表明预测误差越大。

第二个公式为更新状态的，根据前一时刻的预测值结合当前时刻的增量来更新当前时刻的状态。

第三个公式为预测误差更新，利用当前时刻的测量误差和前一时刻的预测误差来计算，常用后面那个推导的更新公式。（这个公式来源？推导可参考秦永元的《卡尔曼滤波与组合导航原理》）

备注：KG越小，此时测预误差比较大，预测误差更新越缓慢，保证逐步收敛，不出现跳跃。

Lecture 5: A Simple Example of The Kalman Filter Numerical temperature example

下图是一个温度估计的例子，展现了KF的三个阶段的计算过程。

反复迭代以上过程，预测误差会越来越小。

Lecture 6: A Simple Example of The Kalman Filter(Continued)

下图是一个更加详细的推导例子，从图中的推导过程可以看出，经过4次迭代后，预测值由68变化到71，逐渐接近真实值72。可以看出，KF的收敛速度是比较快的。

Lecture 7: The Multi-Dimension Model 1

下图为KF的具体流程公式，首先给定一个状态和预测误差矩阵初值，然后进入图中央的迭代循环过程。

第一步（PREVIOUS STATE）：将当前时刻的更新为前一时刻的状态和误差。

第二步（NEW STATE(PREDICTED)）：利用系统模型更新当前时刻的预测状态矩阵和预测误差矩阵，其中A、B都是系数矩阵，为控制方差矩阵，为预测状态噪声矩阵，Q为处理噪声的协方差矩阵。上述系数、矩阵的含义以及在实际中如何选取Question？？？常用运动学模型 

第三步（UPDATE MEASUREMENT AND KALMAN GAIN）：计算KF增益K以及根据第二步的预测值和当前时刻的测量值进行融合得到当前时刻的最优估计状态。其中测量值此时的系数矩阵C代表预测值与测量值之间的转换关系，实际应用中如何体现? (两个例子 1.和等效 状态矩阵可以直接测量 2.通过某种关系建立)

第四步（CURRENT BECOMES PREVIOUS）：进行预测误差矩阵更新，输出当前最优估计结果。

备注：实际运用中RTKLIB开源库预测方程常常是用上一次的定位结果，量测方程需要根据实际情况建立。

Lecture 8: The Multi-Dimension Model 2 –The State Matrix

状态矩阵指的是我们感兴趣的、想要估计的参数，其形式通常如，并且通过预测（状态）方程来更新。其中为外加控制变量矩阵，为过程噪声矩阵，其更新周期一般越小越好（RTKLIB定位里直接用上一秒的定位结果，原因可能是太小，即当前时刻和前一时刻位置一致）。实际应用过程中如何建立预测方程？动力学方程

Lecture 9: The Multi-Dimension Model 3 – The State Matrix

该节举了三个例子，根据动力学定律建立预测方程和状态矩阵X，系数矩阵A一样，因此其更新结果也类似（此时不考虑后面的外加控制变量矩阵以及噪声矩阵）。

Lecture 10: The Multi-Dimension Model 4 –The State Matrix

此时考虑外加控制变量矩阵，那么不同场景其系数矩阵B会发生变化。如何选择B？分清楚状态矩阵和控制变量矩阵uk的关系？动力学方程建立

Lecture 11: 5: Find The State Matrix Of A Falling Object

下图以一个FALLING OBJECT 为例子，对状态矩阵Xk的更新进行了详细的推导。

Lecture 12: 6: Update The State Matrix

下图展示了观测量如何通过测量方程与状态矩阵建立联系的，图中系数矩阵C取值有点奇怪, 观测量直接获得。实际应用中需要仔细考虑这个方程如何从抽象的问题中建立。

Lecture 13: 7: State Matrix Of Moving Object In 2-D

下图展示了一个移动物体的二维状态矩阵的系统状态预测过程（不涉及）

Lecture 14: 8: What Is The Control Variable Matrix?

在上述例子的基础上，考虑在预测方程中。

Lecture 15: 9: Converting From Previous To Current State 2-D

结合上述两个例子，将预测方程的各个部分放在一起，合成2D运动物体的系统状态预测方程的实际应用。

Lecture 16: 10: Converting From Previous To Current State 3-D

三维状态矩阵的预测方程例子，具体表达形式如下：

Lecture 17: 11: Numerical Ex. Of Finding The State Matrix 1-D

下图展示了一个具体的例子，给出初值，计算迭代的结果。

Lecture 18: What Is A Covariance Matrix?

下图主要讲述了和的推导。协方差矩阵有两种，状态协方差矩阵P和测量协方差矩阵R，可以理解为误差矩阵。它们可以推导出KALMAN增益K来对预测和量测二者进行权衡（更相信谁）。

备注：状态协方差矩阵P中存在过程噪声矩阵Q，是为了防止P的更新出现零的特殊情况，其次实际情况中也会有噪声。but 如何取值？？？经验值

Lecture 19: What Is A Variance-Covariance Matrix?

下图定义了均值、方差、协方差等的基本概念，从1D、2D到3D协方差矩阵的模板。

Lecture 20: Example Of Covariance Matrixn And Standard Deviation

方差表示其离平均值的波动范围，也代表测量值与真实值的误差。从高斯模型可以看出，覆盖数据的概率越来越大。

Lecture 21: Finding The Covariance Matrix, Numerical Ex. 1

下图给出了一个计算协方差矩阵的实际例子，此处只有主对角线上方差的计算：

Lecture 22: Finding The Covariance Matrix, Numerical Ex. 2

下图计算为计算协方差的实例，接上图，形成完整的协方差矩阵。

Lecture 23: Finding The Covariance Matrix, Numerical Example

一个计算测量协方差矩阵的例子，一种技巧-以矩阵操作的思维来计算协方差矩阵。

备注：协方差矩阵的元素大小表明-->正相关 同向变化 比如数学与物理成正相关，数学成绩高 物理也高。

Lecture 24: Finding The State Covariance Matrix: P=?

状态协方差矩阵的简单例子，此时副对角元素计算为 ，这是一个特殊情况 互相关系数为1，两个状态线性相关。

Lecture 25: Explaining The State Covariance Matrix

下图解释了状态协方差矩阵代表的含义-主对角线为估计状态的误差，副对角线为状态的相关性，为0以为二者相互独立。

Lecture 26: Flow Chart Of 2-D Kalman Filter - Tracking Airplane

下图为追踪飞机的例子，介绍了基本的流程框图 给定飞机的位置、速度、加速度、测量值、各种误差等。

备注：C如何取？ 第五步？-->实际问题

Lecture 27: 1. The Predicted State -Tracking Airplane

第一步 根据运动规律 进行预测方程建立 迭代更新第一次

Lecture 28: 2. Initial Process Covariance- Tracking Airplane

下图为状态（过程）误差矩阵的计算，实际应用中副对角线上的元素通常简化为0，因为此时这两个状态之间没什么联系，我们主要关注方差（主对角线元素），它代表了状态的误差。

Lecture 29: 3. Predicted Process Covariance - Tracking Airplane

下图根据初值，计算更新过程误差矩阵Pk，Q此时默认取0。

备注：具体含义？maybe need推导

Lecture 30: 4. Calculate The Kalman Gain - Tracking Airplane

下图为计算KALMAN滤波的增益K，实例中转换矩阵H取单位阵。其计算结果表明此次更新K = 0.405比较小，测量误差较大（25>20），选择更加相信预测值。

备注：转换矩阵代表将误差矩阵P转换到增益矩阵K（物理意义？）。

Lecture 31: 5. The New Observation -Tracking Airplane

下图为测量方程的例子，此例中直接观察出状态量，因此系数矩阵C为单位阵，观测噪声矩阵取0。

备注：实际问题中预测方程的模型究竟如何建立？有两个例子 1.直接用上一秒的结果 2.动力学方程预测

如何保证其迭代后预测更加准确？高斯函数G(x)性质 两个G(x)相乘，其方差会变小，即误差变小。

其次，实际中的观测量可能是间接量，需要建立关系。

Lecture 32: 6. Calculate Current State -Tracking Airplane

下图表明了如何更新当前状态矩阵，公式如下，当K比较大时，会有更多的增量

来更新当前的最优估计，从结果可以看出，的值比较偏向预测值。

Lecture 33: 7. Update Process Covariance - Tracking Airplane

下图表明了更新过程协方差矩阵

Lecture 34: 8. Current Becomes Previous -Tracking Airplane

下图为获取当前的最优估计后，把当前状态更新为上一秒状态，为下一步预测做准备。

Lecture 35: 1, 2, 3 Of Second Iteration -Tracking Airplane

下图为第二次迭代的 预测系统状态方程和过程协方差矩阵的更新。

Lecture 36: 4. Kalman Gain Second Iteration - Tracking Airplane

下图计算第二次迭代过程中的KALAM增益矩阵K的计算。

备注：K为一个对角阵。

Lecture 37: 5, 6 Of Second Iteration - Tracking Airplane

下图为第二次迭代更新过程中，利用预测值和观测值结合KALMAN增益来更新当前状态矩阵。

Lecture 38: 7, 8 Of Second Iteration - Tracking Airplane

第二次迭代过程 更新状态误差矩阵，并将其和状态矩阵更新为上一秒数据，为一步迭代做准备。

Lecture 39: Part 1 Of Third Iteration - Tracking Airplane

第三次迭代更新过程 获取当前的状态预测矩阵状态协方差矩阵KALMAN增益。

Lecture 40: Part 2 Of Third Iteration - Tracking Airplane

下图为第三次迭代过程中的当前最优状态估计计算过程，并更新当前的最优状态协方差矩阵，为下一次迭代做准备。

Lecture 41: Graphing 1St 3 Iterations (T Vs X) - Tracking Airplane

下图为上述例子的时间和位移的图形化表示，从图中我们可以看出KALMAN滤波就是在预测值与测量值之间根据误差大小进行折中处理。

Lecture 42: Graphing 1St 3 Iterations (T Vs V) - Tracking Airplane

下图为速度与时间的关系，同上

备注：KALMAN滤波值的输出是最大概率上符合真实值的，因此经过三轮迭代后的位移和速度的结果都比较准确。