最简单的卡尔曼滤波示例（naive demo for Kalman Filter）
我们先抛开历史简介，抛开数学推导，先来看看最简单的Kalman Filter的示例(如下图所示)：
卡尔曼滤波简介(intro to Kalman Filter)
Kalman filter(卡尔曼滤波)，使用一系列”包含统计噪声和不确定性的观测值”，来估计未知的状态的联合概率分布，这种算法比单独使用一个量测估计要准确。好的，文绉绉的定义的翻译到此为止，现在用大白话来翻译一下Kalman Filter的示例：
- 蓝色：机器人真正的”位置”
- 绿色：机器人，待估计的状态仅包含“位置”（即在x轴上的坐标）和”位置的不确定度”
- 黑色：门，作为地图，这里是已知信息，门的不确定度事实上是机器人的测量误差
- 灰色：机器人位置的概率分布的熵（熵代表混乱程度，越小越好，越大越混乱/不可信）
- 额外说明：这里的“位置”和其“误差”（不确定度）用高斯分布表示，“位置”即“均值”，“误差”即“方差”；高斯分布会在这个示例之后简要介绍
- 示例代码
一开始，机器人在0的位置，因为x轴的0处有一个标识，因此有一个很高的置信度；但是，除了0刻度以外，就没有其他刻度了，因此需要我们对机器人的“位置”进行估计/计算。
假设我们对机器人设定好了程序，让它以0.5m/s的速度，向x正轴前行，因为我们用的传感器比较差，虽然程序告诉它了速度，但传达到轮子最终执行的时候，总会有那么点误差（不确定度），这里假设误差为0.04(m/s)^2；
假设我们在机器人上检测了“检测门”的传感器，当这个传感器距离门小于0.01m的时候，可以检测到门的存在；
假设在x正轴方向上有5扇一模一样的门，我们事先通过测距的方式得到了各个门的精确位置，假设地图里的位置完全没有误差，但是机器人身上的传感器比较廉价，测量时会有较大的误差。
好的，机器人开始运行，由于没有外界信息的输入，机器人虽然以为自己0.5m/s的速度匀速前行，但是不确定度的存在，导致它现在“位置”变得越来越不可信，正如图中所示，真正可能存在的位置越来越宽；我们把这个过程称之为“预测”（predict）。
还好，我们帮它提前准备好了“地图”，当它遇到第一个门的时候，虽然门的不确定度也不低，但是由于高斯分布的特性（后续解释），两个高斯分布在一起产生了不可思议的“化学反应”，机器人的不确定度神奇地降低了；我们把这个过程称之为量测“更新”（update），其中，量测是指“检测”到门，并从事先的地图中拿到了门的信息，融合进机器人的位置估计/计算中，使得机器人的“位置”更加可信。
之后的故事就是“重复之前的故事”，没有额外信息输入的时候，机器人的不确定度越来越大，又遇到门的时候，不确定度骤降……
小结一下：卡尔曼滤波，就是不断重复“预测”和“更新”的过程，“预测”由于没有额外信息输入，误差（不确定度）不断增大；“更新”由于有额外信息输入，误差（不确定度）骤降；
高斯函数简介
我们这里就不照搬高斯函数/正态分布的定义了，结合示例，我们用一维的情况介绍一下高斯函数：
制作地图的时候，假设我们用的精度较差的激光测距仪测量门的坐标x，每一扇门我们都测量10次，然后把这10次的测量结果求平均得到mean，求方差得到variance，其中，mean即我们最终使用的门的位置，variance则是表达该门的位置的不确定度。
理想情况下，这些门的不确定度（variance）完全相同，因为所有门都是一模一样的，测量仪器也是同一个。
方差对比
上图给出了四个不同的高斯分布的示例，均值mean控制曲线的中间位置，方差variance控制曲线的张口大小；张口越小，说明符合该分布的数据点越集中，得到的结果越可信；张口越大，说明该分布的数据点越发散，得到的结果越不可信；
对于机器人的位置估计/计算来说，我们肯定是希望张口越小越好。
高斯函数的计算
给定均值mean、方差variance和某点x，带入上式即可求x得概率密度，例如：
mean = 10，variance = 4， x = 8， P(x) = 0.120985
我们这里不给出数学推导，从上图示例就可以看出来，当x = mean的时候，P(x)最大；
运动预测
运动预测本质上是“全概率”的“卷积”过程，这个会放在后续的“无参定位”部分来讲，这里直接给出结论：
“位置”（均值）预测：
“不确定度”（方差）预测：
本质上就是“位置”（均值）相加，“不确定度”（方差）变大，因为运动预测没有额外信息输入，运动给机器人带来了新的不确定度，因此，这个公式是符合直觉的。
量测更新
量测更新本质上是基于“贝叶斯规则”的概率相乘，这个也会放在后续的“无参定位”部分来讲，这里直接给出结论：
“位置”（均值）更新：
“不确定度”（方差）更新：
本质上是对现在的“位置”（均值）和量测得到的”位置“（均值）根据它们的”不确定度“做了”加权平均“；例如，如果量测值更可信，即量测值得不确定度更小，那么之前预测得到的“位置”就更不可信，这也是符合直觉的。不符合直觉的是不确定度，根据上面的“不确定度”（方差）更新公式，得到的新的“不确定度”（方差）要比旧的和量测值的不确定度都要小，这就是卡尔曼滤波的神奇之处！
自动驾驶
同样的图，不一样的说明，我们再来看看最简单的Kalman Filter的示例在自动驾驶中的应用(如下图所示)：
- 蓝色：自动驾驶汽车真正的位置
- 绿色：自动驾驶汽车，待估计的状态包含“位置(x, y, z)”，“姿态(yaw, pitch, roll)”，“速度(v_x, v_y, v_z)”，下文用“状态”一词代替这些变量，以及方圆100m的各个动态物体的“位置(x, y, z)”，“姿态(yaw, pitch, roll)”，“速度(v_x, v_y, v_z)”
- 黑色：地图特征，这里是已知信息，包括“电线杆”、“交通标志”、“地面标记”、“车道线”等等，一切可以用来辅助定位的信息，都可以提前采集放入地图中
- 灰色：自动驾驶汽车位置的概率分布的熵（熵代表混乱程度，越小越好，越大越混乱/不可信）
- 示例代码
“计算平台”负责各个“模块”的资源调度，可以理解成自动驾驶的“操作系统”，“计算平台”利用GPS或者其他传感器信息，得到初始位置，并加载该区域的特征地图（用于定位）和逻辑地图（用于规划路径）
设定好终点后，“计算平台”在“逻辑地图”中搜索规划各种可能的到达终点的路径，结合当各条路径的路况，可能需要的时间等因素，给出最优的路径；
运行过程中，“定位模块”利用轮速计、IMU（惯性测量单元）等传感器“预测”当前自动驾驶汽车的“状态”，同时利用相机、激光雷达等传感器检测特征，与“特征地图”中已有的特征做对比，提供量测“更新”。“感知模块”检测行人、车辆等各种障碍物，“规划模块“根据“定位模块”提供的状态，“感知模块”提供的信息，结合“逻辑地图”的信息，“规划”之后xx米的路径，“控制”则根据”规划模块“的”规划”，“控制”无人驾驶汽车加速/减速、直行/拐弯、开转向灯、按喇叭等；
到此，卡尔曼滤波在自动驾驶中的应用，就算科普完毕啦！
