【无人驾驶笔记2.4】Session 2.4 激光，雷达传感器与卡尔曼滤波 Udacity无人驾驶 无人车笔记

2.4主要讲了卡尔曼滤波和激光&雷达系统的联动，主讲的是奔驰团队的羞涩小哥。例子里面这个系统主要考虑追踪人。其实车应该也是可以的。

左边胖哥似乎是老板，中间是羞涩小哥

扯个题外话这个课里面居然有David Silver感觉卡司阵容牛逼爆了，以及他好像比原来胖一些。。

从overview上来看一个滤波系统差不多是这样的。其实就是一个online的tracking系统。如果Lidar和Radar系统发现一个物体的时候，就会判断这个物体是不是一个新的物体。如果是的话就会初始化一套新的状态变量（新物体的位置和方差）。不然就会套用Kalman滤波体系更新旧有物体的状态。

这个session所讲系统的pipeline

这里我们重新复习一下上次笔记讲得很不全面的贝叶斯滤波系统。贝叶斯滤波总是会维护一个状态，过了一个Delta T时间之后，会根据以前的状态（参考原来的位置和速度）去估计当前的状态。其实这个过程在比较简单的模型里面就是随机游走。如果只随机游走时间长了状态就会扩散到全空间。 然后就是测量过程，会根据某种传感器获得一个观测，根据这个观测去修正我们的‘信念’。基本就是这样。

Bayes 滤波

另外Radar的读数是基于极坐标的，观测和状态之间的函数是一个非线性的函数。所以这个时候需要将Kalman滤波扩展为 扩展卡尔曼滤波 （我随便起的翻译不知道是不是这样的，反正就是EKF）。其实就是对非线性函数局部线性化来期望依旧使用卡尔曼滤波。

其实我觉得多这个名词完全没必要不就是Taylor展开嘛。。

考虑多种传感器

这张slides是对之前Kalman滤波模型的一个回顾

需要注意的是，这次的Kalman滤波会同时考虑两种不同的传感器。另外会带来一个问题。两个传感器的测量间隔Delta t是任意的。这个任意性会影响到运动模型里面对于协方差的估计。肯定时间越长方差应该越大，那么间隔时间和方差的关系究竟如何呢。如果我们把运动模型想象成一个匀加速度模型，对比原来的模型，其实加速度的项就是误差项需要考虑的。

那么其实我们可以推出那个加速度相关随机变量的协方差矩阵。他这个模型首先假设x y方向的协方差。然后中间那个Q矩阵是靠统计设定的。或者调参设定。

有了运动模型我们就要考虑观测模型。

一个极度化简的观测pipeline

激光雷达接收到的是点云。他这里直接省略了非常复杂的物体检测过程。假设我们可以直接检测到一个物体的位置。（这里实际还包含了一个假设，就是这个物体就是我们要更新的物体）

激光雷达的观测矩阵

状态变量是位置和速度。所以激光雷达的观测模型颇为简单。就是个对角阵。

然后就可以做模拟了。绿色是（目标行人）的实际轨迹。红色是激光雷达的观测。蓝色是卡尔曼滤波的最终结果。

增大R的效果

在这里如果试着增大测量误差的估计。会发现轨迹偏离ground truth。这实际上也说明了匀速运动这个模型构造得不好。目标其实在做类似圆周运动的话。匀速运动模型假设的kalman滤波就会出现欠拟合。其实kalman滤波调参还是很重要的。不过其实也可以用MSE的方法来学习。另外在2.5会讲另外一个匀速圆周运动的模型。那个模型对于这种曲线路径的拟合显然要更好

两种雷达的互补

激光雷达可以直接得到位置，不过对于速度的感知能力较弱。这个时候雷达就可以补足这方面的感知。

雷达的观测模型

雷达的观测模型能够得到物体面向传感器的速度投影（多普勒效应），角度，和距离。话说其实很多导弹里面的末端制导和引信也用了类似的东西（哎呀我废话好多啊）

雷达的观测函数

这个观测模型就是非线性的了，并不能直接套用之前线性的卡尔曼滤波模型。老师还解释了一个非线性函数映射高斯随机变量，像变量不再是高斯分布。不过反正做个线性化就可以了，这就是EKF

真观测雅可比矩阵

后面好像放了很多demo视频

Castro ST Demo

我觉得比较神奇的是这个东西可以分割出很多bounding box块儿对这些块儿进行追踪。

其实这个session的内容基本就是那么多。不过其实省去了一个很重要的topic，就是多传感器的时候，怎么detect & track & identity 一个物体。这个部分其实是很重要的。光看raw data的话其实是没有办法知道这些的。不过也可能可以用最近邻的方法来先实现最naive版本的track和identity。这样只要每个传感器下面都有detector，那么就可以自然套用这个框架。还有一种可能是可以构造更复杂的观测模型。比如在雷达下面我不做detector，但是我知道这个雷达区域有一个物体的时候，雷达的返回应该是什么样的，构造一个更复杂的观测模型，应该也是可以的。