优达学城《无人驾驶入门》学习笔记—

优达学城《无人驾驶入门》的第二个项目是实现矩阵类，要求通过python编写一个用来计算矩阵的类Matrix.编写这个类并不难，涉及到的线性代数方面的知识也不多，比如矩阵的加法、减法、乘法，求逆矩阵，创建单位矩阵等。相比之下，理解编写矩阵类的目的反而显得更加重要。

编写矩阵类，是为实现了卡尔曼滤波器。卡尔曼滤波器涉及的知识很广，涵盖了《无人驾驶入门》课程第二部分“贝叶斯定理”和第三部分“使用矩阵”这两部分的内容，包括贝叶斯定义、高斯分布、运动模型、线性代数等内容。个人认为，如果把“实现卡尔曼滤波器”做为项目会更好。可能考虑到难度有些大，课程是通过workspace（“卡尔曼滤波器和你的矩阵类”）的形式演示了如何实现卡尔曼滤波器。workspace还调用了datagenerator用来生成输入的数据，它是非常好的学习资源，值得研究。

这篇文章的目的有2个：

第一个目的，介绍实现卡尔曼滤波器的3个步骤：

创建矩阵类。编写矩阵类是你需要完成的项目，我不会在这里公布答案，而是向你介绍一些扩展内容。项目要求使用列表来实现矩阵类，这样做的好处是，首先，需要的python知识比较基础，难度不大；其次，了解矩阵计算的原理。作为扩展内容，我会提供一个新思路，向你介绍如何通过numpy库来实现矩阵类。使用numpy库的ndarray和matrix来计算矩阵，会比列表方便得多。丰富的第三方库，正是python功能强大的原因之一。
创建汽车行驶的数据。我会对workspace“卡尔曼滤波器和你的矩阵类”中调用的datagenerator.py文件进行讲解，介绍函数generate_data和generate_lidar是如何生成汽车的行驶数据和测量数据的。这两个函数运用到了课程中学习过的运动模型和高斯分布等知识。
实现卡尔曼滤波器，并且可视化。把卡尔曼滤波器的公式编写成程序并不难，但是我希望你能了解公式中的参数分别代表哪些量，如何获得这些量。至于如何推导公式，如果有能力完成当然更好，推导不出来也没有关系。最后，通过matplotlib库将卡尔曼滤波器可视化。

第二个目的，推荐一些学习资料。主要包括numpy库，matplotlib库，卡尔曼滤波器的公式推导，以及一个神奇的公式可视化网站，你可以用来观察高斯分布等公式。

1 实现卡尔曼滤波器的步骤

1 创建矩阵类

numpy库的ndarry和matrix对象非常适合用来实现矩阵计算。两个列表相加，是这样实现的：

  a = [1,2,3]b = [4,5,6]print(a+b)[1, 2, 3, 4, 5, 6]

而两个长度相同的ndarray对象相加，结果完全不同：

  c = np.array([1,2,3])d = np.array([4,5,6])print(c+d)[5 7 9]

很显然，ndarray的加法就是矩阵的加法。ndarray还有很多矩阵计算的方法和属性，比如矩阵乘法：ndarray.dot()；矩阵的迹：ndarray.trace()；转置矩阵：ndarray.T.

matrix对象大部分功能和ndarray相同，但是它还提供了一个计算逆矩阵的方法：Matrix.I.

除此之外，numpy还提供了创建矩阵的函数，比如创建单位矩阵：numpy.eye().

下面是通过numpy类实现矩阵类的代码。如果你已经完成了这个项目，对比一下，你会发现，numpy库可以让代码简洁不少。

 import numbersimport numpy as npimport matplotlib.pyplot as plt  import math# 卡尔曼滤波器需要调用的矩阵类class Matrix(object):# 构造矩阵def __init__(self, grid):self.g = np.array(grid)self.h = len(grid)self.w = len(grid[0])# 单位矩阵def identity(n):return Matrix(np.eye(n))# 矩阵的迹def trace(self):if not self.is_square():raise(ValueError, "Cannot calculate the trace of a non-square matrix.")else:return self.g.trace()# 逆矩阵def inverse(self):if not self.is_square():raise(ValueError, "Non-square Matrix does not have an inverse.")if self.h > 2:raise(NotImplementedError, "inversion not implemented for matrices larger than 2x2.")if self.h == 1:m = Matrix([[1/self[0][0]]])return mif self.h == 2:try:m = Matrix(np.matrix(self.g).I)return mexcept np.linalg.linalg.LinAlgError as e:print("Determinant shouldn't be zero.", e)# 转置矩阵def T(self):T = self.g.Treturn Matrix(T)# 判断矩阵是否为方阵def is_square(self):return self.h == self.w# 通过[]访问def __getitem__(self,idx):return self.g[idx]# 打印矩阵的元素def __repr__(self):s = ""for row in self.g:s += " ".join(["{} ".format(x) for x in row])s += "\n"return s# 加法def __add__(self,other):if self.h != other.h or self.w != other.w:raise(ValueError, "Matrices can only be added if the dimensions are the same")else:return Matrix(self.g + other.g)# 相反数def __neg__(self):return Matrix(-self.g)#减法def __sub__(self, other):if self.h != other.h or self.w != other.w:raise(ValueError, "Matrices can only be subtracted if the dimensions are the same")else:return Matrix(self.g - other.g)# 矩阵乘法：两个矩阵相乘def __mul__(self, other):if self.w != other.h:raise(ValueError, "number of columns of the pre-matrix must equal the number of rows of the post-matrix")    return Matrix(np.dot(self.g, other.g))# 标量乘法：变量乘以矩阵          def __rmul__(self, other):if isinstance(other, numbers.Number):return Matrix(other * self.g)

2 创建汽车行驶的数据

实现卡尔曼滤波器，需要汽车的行驶数据。行驶数据分为两类，一类是真实数据，另一类是传感器测量到的数据。

真实数据通过建立汽车的运动模型来实现。测量数据需要考虑传感器的误差，假设误差呈高斯分布，那么，可以在真实数据的基础上加上一个高斯分布的样本，样本的期望为0，标准差为0.15.

真实数据是通过函数generate_data()来实现的。为了突出重点，我对函数进行了简化，比如删除了单位换算，一律使用国际单位制；简化了运动模型。

 # 生成汽车行驶的真实数据# 汽车从以初速度v0，加速度a行驶10秒钟，然后匀速行驶20秒# x0:initial distance, m# v0:initial velocity, m/s# a:acceleration，m/s^2# t1:加速行驶时间，s# t2:匀速行驶时间，s# dt:interval time, sdef generate_data(x0, v0, a, t1, t2, dt):a_current = av_current = v0t_current = 0# 记录汽车运行的真实状态a_list = []v_list = []t_list = []# 汽车运行的两个阶段# 第一阶段：加速行驶while t_current <= t1:# 记录汽车运行的真实状态a_list.append(a_current)v_list.append(v_current)t_list.append(t_current)# 汽车行驶的运动模型v_current += a * dtt_current += dt# 第二阶段：匀速行驶a_current = 0while t2 > t_current >= t1:# 记录汽车运行的真实状态a_list.append(a_current)v_list.append(v_current)t_list.append(t_current)# 汽车行驶的运动模型t_current += dt# 计算汽车行驶的真实距离x = x0x_list = [x0]for i in range(len(t_list) - 1):tdelta = t_list[i+1] - t_list[i]x = x + v_list[i] * tdelta + 0.5 * a_list[i] * tdelta**2x_list.append(x)return t_list, x_list, v_list, a_list# 生成雷达获得的数据。需要考虑误差，误差呈现高斯分布def generate_lidar(x_list, standard_deviation):return x_list + np.random.normal(0, standard_deviation, len(x_list))# 获取汽车行驶的真实状态t_list, x_list, v_list, a_list = generate_data(100, 5, 4, 10, 20, 0.1)# 创建激光雷达的测量数据# 测量误差的标准差。为了方便观测，可以增加该值。standard_deviation = 0.15# 雷达测量得到的距离lidar_x_list = generate_lidar(x_list, standard_deviation)#　雷达测量的时间lidar_t_list = t_list

  真实数据和测量数据生成完毕后，下面将这些数据可视化。一共会创建6幅图像，分别是真实距离，真实速度，真实加速度，激光雷达测量的距离值。

  # 可视化.创建包含2*3个子图的视图fig, ((ax1, ax2, ax3), (ax4, ax5, ax6)) = plt.subplots(2, 3, figsize=(20, 15))# 真实距离ax1.set_title("truth distance")ax1.set_xlabel("time")ax1.set_ylabel("distance")ax1.set_xlim([0, 21])ax1.set_ylim([0, 1000])ax1.plot(t_list, x_list)# 真实速度ax2.set_title("truth velocity")ax2.set_xlabel("time")ax2.set_ylabel("velocity")ax2.set_xlim([0, 21])ax2.set_ylim([0, 50])ax2.set_xticks(range(21))ax2.set_yticks(range(0, 50, 5))ax2.plot(t_list, v_list)# 真实加速度ax3.set_title("truth acceleration")ax3.set_xlabel("time")ax3.set_ylabel("acceleration")ax3.set_xlim([0, 21])ax3.set_ylim([0, 5])ax3.plot(t_list, a_list)# 激光雷达测量结果ax4.set_title("Lidar measurements VS truth")ax4.set_xlabel("time")ax4.set_ylabel("distance")ax4.set_xlim([0, 21])ax4.set_ylim([0, 1000])ax4.set_xticks(range(21))ax4.set_yticks(range(0, 1000, 100))ax4.plot(t_list, x_list, label="truth distance")ax4.scatter(lidar_t_list, lidar_x_list, label="Lidar distance", color="red", marker="o", s=2)ax4.legend()

可以通过plt.show()观察一下。

3 实现卡尔曼滤波器，并且可视化

卡尔曼滤波器的公式如下：

预测：

$x' = Fx + Bu$

$P' = FPF^T + Q$

更新：

$y = z - Hx'$

$S = R + HP'H^T$

$K = P'H^TS^{-1}$

$x = x' + Ky$

$P = (I - KH)P'$

$y = z - Hx$

下面简单介绍一下各个参数的含义。如果想深入了解公式的推导过程，可以看看最后推荐的文章。

预测状态： $x' = Fx + Bu$

根据本时刻的状态 $x$ （这里是位置和速度），基于状态转换矩阵 $F$ ，控制矩阵 $B$ ，以及控制向量 $u$ ，预测下一时刻的状态 $x'$ .

预测误差的协方差矩阵： $P' = FPF^T + Q$

状态向量中的两个变量p（位置）和v（速度）存在相关性，用误差协方差矩阵 $P$ 表示。外部无法检测到的干扰，比如风的干扰，也会增加不确定性。把这些没有被跟踪的干扰当作协方差为的噪音来处理。 $Q$ 是过程噪音协方差矩阵，它会增加不确定性。

预测状态和测量状态之差 ： $y = z - Hx'$

z是测量向量，表示传感器测量到的数据（比如传感器测量到的汽车的位置）。测量值和预测值（更新后的）之间存在误差 $y$ .

如果状态向量和测量向量包含的数据不同，比如状态向量包括位置和速度，而测量向量只有位置，那么 $H = [[1, 0]]$ 。

测量噪音协方差矩阵： $R$

测量结果存在噪音，用 $R$ 来表示测量噪音协方差矩阵。因为测量值只有位置一个变量，所以这里是位置的方差。

用于计算卡尔曼增益的中间量： $S$

$S = R + HP'H^T$

卡尔曼增益： $K = P'H^TS^{-1}$

更新状态矩阵： $x = x' + Ky$

更新误差协方差矩阵： $P = (I - KH)P'$

各个变量之间的关系请见下图：

这张图片来自文尾推荐的推导卡尔曼滤波器公式的文章，有兴趣可以了解一下。

代码如下：

  # 使用卡尔曼滤波器# 初始距离。注意：这里假设初始距离为0，因为无法测量初始距离。initial_distance = 0# 初始速度。注意：这里假设初始速度为0，因为无法测量初始速度。initial_velocity = 0# 状态矩阵的初始值x_initial = Matrix([[initial_distance], [initial_velocity]])# 误差协方差矩阵的初始值P_initial = Matrix([[5, 0], [0, 5]])# 加速度方差acceleration_variance = 50# 雷达测量结果方差lidar_variance = standard_deviation**2# 观测矩阵，联系预测向量和测量向量H = Matrix([[1, 0]])# 测量噪音协方差矩阵。因为测量值只有位置一个变量，所以这里是位置的方差。R = Matrix([[lidar_variance]])# 单位矩阵I = Matrix.identity(2)# 状态转移矩阵def F_matrix(delta_t):return Matrix([[1, delta_t], [0, 1]])# 外部噪音协方差矩阵def Q_matrix(delta_t, variance):t4 = math.pow(delta_t, 4)t3 = math.pow(delta_t, 3)t2 = math.pow(delta_t, 2)return variance * Matrix([[(1/4)*t4, (1/2)*t3], [(1/2)*t3, t2]])def B_matrix(delta_t):return Matrix([[delta_t**2 / 2], [delta_t]])# 状态矩阵x = x_initial# 误差协方差矩阵P = P_initial# 记录卡尔曼滤波器计算得到的距离x_result = []# 记录卡尔曼滤波器的时间time_result = []# 记录卡尔曼滤波器得到的速度v_result = []for i in range(len(lidar_x_list) - 1):delta_t = (lidar_t_list[i + 1] - lidar_t_list[i]) # 预测F = F_matrix(delta_t)Q = Q_matrix(delta_t, acceleration_variance)# 注意：运动模型使用的是匀速运动，汽车实际上有一段时间是加速运动的x_prime = F * xP_prime = F * P * F.T() + Q# 更新# 测量向量和状态向量的差值。注意：第一个时刻是没有测量值的，# 只有经过一个脉冲周期，才能获得测量值。y = Matrix([[lidar_x_list[i + 1]]]) - H * x_primeS = H * P_prime * H.T() + RK = P_prime * H.T() * S.inverse()x = x_prime + K * yP = (I - K * H) * P_primex_result.append(x[0][0])v_result.append(x[1][0])time_result.append(lidar_t_list[i+1])

最后，把卡尔曼滤波器的结果可视化。

  # 把真实距离、激光雷达测量的距离以及卡尔曼滤波器的结果（距离）可视化ax5.set_title("Lidar measurements VS truth")ax5.set_xlabel("time")ax5.set_ylabel("distance")ax5.set_xlim([0, 21])ax5.set_ylim([0, 1000])ax5.set_xticks(range(0, 21, 2))ax5.set_yticks(range(0, 1000, 100))ax5.plot(t_list, x_list, label="truth distance", color="blue", linewidth=1)ax5.scatter(lidar_t_list, lidar_x_list, label="Lidar distance", color="red", marker="o", s=2)ax5.scatter(time_result, x_result, label="kalman", color="green", marker="o", s=2)ax5.legend()# 把真实速度、卡尔曼滤波器的结果（速度）可视化ax6.set_title("Lidar measurements VS truth")ax6.set_xlabel("time")ax6.set_ylabel("velocity")ax6.set_xlim([0, 21])ax6.set_ylim([0, 50])ax6.set_xticks(range(0, 21, 2))ax6.set_yticks(range(0, 50, 5))ax6.plot(t_list, v_list, label="truth velocity", color="blue", linewidth=1)ax6.scatter(time_result, v_result, label="Lidar velocity", color="red", marker="o", s=2)ax6.legend()plt.show()

参考源码：kalman filter

运行代码，得到的图像如下：

为了更好地了解各个参数的意义，我们可以尝试修改一些参数，观察图像的变化。

首先看看激光雷达的测量误差。激光雷达的标准差standard_deviation是0.15，真实数据和测量数据的图像是上图中的第4幅，可以看点表示测量数据的离散点分布在表示真实数据的折线的周围。

如果改变标准差standard_deviation，比如从0.15增加到15，离散点的分散程度就明显多了。

接下来看看真实数据，测量数据和卡尔曼滤波器测量到的距离，也就是第5幅图。当标准差standard_deviation是0.15时，3种数据很难看出差别。

把标准差standard_deviation增加到15，差别就比较明显了。

观察这两幅图，可以得出两个结论：

第一，相比于测量数据，卡尔曼滤波器计算得出的数据更加接近真实数据；

第二，在使用卡尔曼滤波器时，设置的初始距离initial_distance和初始速度initial_velocity都是0，因为初始数据无法测量，所以假设为0.

第三，标准差standard_deviation越大，测量数据越不准确，并且会影响卡尔曼滤波器的精度。比较两幅图可以看出，当标准差standard_deviation是0.15时，卡尔曼滤波器的数据点分布在真实数据周围；而当标准差standard_deviation增加到15时，卡尔曼滤波器的数据点是从0开始的。

第四，初始距离initial_distance和初始速度initial_velocity同样会影响卡尔曼滤波器的测量精度。保持标准差standard_deviation为15不变，把初始距离initial_distance设置为100（同真实距离），初始速度initial_velocity设置为10（同真实速度），可以发现卡尔曼滤波器的结果更加接近真实数据了。

2 荐学习资料推荐

numpy库资料

numpy库官网

网络课程

翻译版

matplotlib库资料

matplotlib库官网

网络课程

翻译版

卡尔曼滤波器公式推导

卡尔曼滤波器公式推导（英文）

卡尔曼滤波器公式推导（中文翻译版）

公式可视化网站

我非常喜欢把抽象的内容可视化，比如数据结构的可视化，算法的可视化，国外也有不少这方面的网站。把抽象的概念具象化，可视化，生动形象，容易理解。在《学习优达学城《无人驾驶入门》，具体需要掌握哪些python知识点？》这篇文章中，我也介绍了一个代码可视化的网站。这次介绍的，是一个公式可视化网站。

DESMOS不但可以显示函数曲线、几何图形，还提供了额外功能，比如，通过改变某一个变量，显示曲线变化的过程。在学习高斯分布这一部分时，我通过改变标准差和期望，观察这两个变量对概率密度曲线的影响。

下面是使用DESMOS需要掌握的最小必要知识：

1 添加滑块

输入自由变量，会提示添加滑块。通过拖动滑块，可以改变图形。

2 添加标签

(x, y)

3 限制范围

{x>0}

4 打开快捷键菜单

CTRL + /

5 查看示例

网站链接：DESMOS

我是《无人驾驶入门》纳米学位的学长，希望这些经验对你有帮助。如果你对udacity的这门课程也感兴趣，可以使用我的优惠码：839662C0，付款时在优惠码框输入，可以抵扣300元学费（限第一次购买udacity课程的学弟学妹用哈）。

优达学城《无人驾驶入门》学习笔记——卡尔曼滤波器实现详解相关推荐

【自动驾驶技术】优达学城无人驾驶工程师学习笔记（六）——Github与Markdown相关教程
Github与Markdown相关教程本博文为笔者关于优达学城无人驾驶工程师课程中计算机视觉基础部分的学习笔记,该部分为实现车道线图像识别功能的基础课程,关于本课程的详细说明请参考优达学城官网. 优 ...
【自动驾驶技术】优达学城无人驾驶工程师学习笔记（七）——计算机视觉基础
计算机视觉基础目录前言颜色选择(Color Selection) 理论基础代码实践区域筛选(Region Masking) 理论基础代码实践 Canny边缘检测问题背景 Canny边缘检测 ...
优达学城，基于激光的卡尔曼滤波器的C++实现。
measurement_package.h #ifndef MEASUREMENT_PACKAGE_H_ #define MEASUREMENT_PACKAGE_H_#include "Ei ...
优达学城Numpy与Pandas笔记
此篇为优达学城数据分析入门第二课笔记网址基本操作 import numpy as np# First 20 countries with employment data countries = np ...
优达学城无人驾驶工程师——P5车辆检测功能
这次讲的是优达学城无人驾驶工程师第一期的最后一个项目,车辆检测功能,代码如下. 导包 import cv2 import numpy as np import matplotlib.pyplot as ...
python人工智能入门优达视频_看优达学城python入门视频学习C++
在如今这个时代,计算机领域中,各种技术瞬息万变,不断更新迭代.随着人工智能.机器学习等先进技术的兴起,近年来,python入门视频是很多人关注的,Python程序员的数量已经大大超过C++程序员.然而 ...
优达学城无人驾驶工程师——P4车道线检测功能
这次讲的是优达学城的无人驾驶工程师的P4项目,利用车前方的摄像头检测车道线,下面开始我们的代码部分. import numpy as np import cv2 import glob import ...
优达学城无人驾驶工程师——P1寻找车道线
这次介绍的是优达学城的无人驾驶工程师的P1项目,利用车的前摄像头来识别当前车道的左右两边两条的车道线.测试图片和视频在文章最后的链接里. 一开始先倒包 #importing some useful p ...
优达学城无人驾驶工程师——P2交通路牌识别
这次是P2项目--交通路牌识别,用到的是简单的卷积网络,2层的卷积层加上4层全连接层,因为用的数据集的图片大小是32x32的,所以不用很复杂的神经网络. 数据地址在这里:https://s3-us-w ...

优达学城《无人驾驶入门》学习笔记——卡尔曼滤波器实现详解

1 实现卡尔曼滤波器的步骤

1 创建矩阵类

优达学城《无人驾驶入门》学习笔记——卡尔曼滤波器实现详解相关推荐

最新文章

热门文章