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

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

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

这篇文章的目的有2个：

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

1. 创建矩阵类。编写矩阵类是你需要完成的项目，我不会在这里公布答案，而是向你介绍一些扩展内容。项目要求使用列表来实现矩阵类，这样做的好处是，首先，需要的python知识比较基础，难度不大；其次，了解矩阵计算的原理。作为扩展内容，我会提供一个新思路，向你介绍如何通过numpy库来实现矩阵类。使用numpy库的ndarray和matrix来计算矩阵，会比列表方便得多。丰富的第三方库，正是python功能强大的原因之一。

2. 创建汽车行驶的数据。我会对workspace“卡尔曼滤波器和你的矩阵类”中调用的datagenerator.py文件进行讲解，介绍函数generate_data和generate_lidar是如何生成汽车的行驶数据和测量数据的。这两个函数运用到了课程中学习过的运动模型和高斯分布等知识。

3. 实现卡尔曼滤波器，并且可视化。把卡尔曼滤波器的公式编写成程序并不难，但是我希望你能了解公式中的参数分别代表哪些量，如何获得这些量。至于如何推导公式，如果有能力完成当然更好，推导不出来也没有关系。最后，通过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 numbers
  import numpy as np
  import 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 m
          if self.h == 2:
              try:
                  m = Matrix(np.matrix(self.g).I)
                  return m
              except np.linalg.linalg.LinAlgError as e:
                  print("Determinant shouldn't be zero.", e)
  ​
      # 转置矩阵
      def T(self):
          T = self.g.T
          return 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)
  ​