手撕自动驾驶算法——卡尔曼滤波KF

令狐少侠、

已于 2022-04-05 15:28:38 修改

阅读量1.4w

点赞数 22

分类专栏：手撕自动驾驶算法文章标签：自动驾驶

于 2019-08-17 11:43:34 首次发布

本文链接：https://blog.csdn.net/weixin_42905141/article/details/99690849

版权

手撕自动驾驶算法专栏收录该内容

22 篇文章 73 订阅

订阅专栏

卡尔曼滤波器简介——追踪介绍

什么是卡尔曼滤波?　　
你可以在任何含有不确定信息的动态系统中使用卡尔曼滤波，对系统下一步的走向做出有根据的预测，即使伴随着各种干扰，卡尔曼滤波总是能指出真实发生的情况

在连续变化的系统中使用卡尔曼滤波是非常理想的，它具有占用内存小的优点（除了前一个状态量外，不需要保留其它历史数据），并且速度很快，很适合应用于实时问题和嵌入式系统。

自动驾驶汽车使用激光雷达跟踪其他车辆，如何找到其他车辆？我想找到其他车辆的原因是我不想和其他车辆发生碰撞。

为了进行评估我们必须得先明白如何解释传感器数据。不知是明白其他车辆在那，还要知道速度是多少，这样你就可以避免与他们进行碰撞，这对行人和其他物体也很重要。

知道其他车在那里，并且对他们将要去那里做出预测。

卡尔曼滤波是一个非常流行的系统状态估计的方法，他和概率定位相当相似，我们之前学过的蒙特卡罗定位方法，主要区别是卡尔曼是对一个连续状态进行估计，而蒙特卡罗将世界分成很多离散的小块，作为结果，卡尔曼给我们一个单峰分布，蒙特卡罗是多峰分布。这两种方法都适用与定位和对其他车辆的追踪。事实上粒子滤波也适用于定位和预测，粒子滤波是连续多峰分布的。

卡尔曼滤波器的美妙之处在于，它将不够准确的传感器测量结果和不够准确的运动预测相结合，得到一个筛选后的位置估计值，这个估计值比所有仅来自传感器读数或运动预测的估计值更好。

1.卡尔曼滤波原理

卡尔曼滤波假设两个变量（位置和速度，在这个例子中）都是随机的，并且服从高斯分布。每个变量都有一个均值 μ，表示随机分布的中心（最可能的状态），以及方差，表示不确定性

1.1 使用矩阵来描述问题

我们基于高斯分布来建立状态变量，所以在时刻 k 需要两个信息：最佳估计（即均值，其它地方常用 μ 表示），以及协方差矩阵。
在这里插入图片描述
（当然，在这里我们只用到了位置和速度，实际上这个状态可以包含多个变量，代表任何你想表示的信息）。接下来，我们需要根据当前状态（k-1 时刻）来预测下一状态（k 时刻）。记住，我们并不知道对下一状态的所有预测中哪个是“真实”的，但我们的预测函数并不在乎。它对所有的可能性进行预测，并给出新的高斯分布。
我们可以用矩阵Fk来表示这个预测过程：

它将我们原始估计中的每个点都移动到了一个新的预测位置，如果原始估计是正确的话，这个新的预测位置就是系统下一步会移动到的位置。那我们又如何用矩阵来预测下一个时刻的位置和速度呢？下面用一个基本的运动学公式来表示：
在这里插入图片描述
现在，我们有了一个预测矩阵来表示下一时刻的状态，但是，我们仍然不知道怎么更新协方差矩阵。此时，我们需要引入另一个公式，如果我们将分布中的每个点都乘以矩阵 A，那么它的协方差矩阵这里写图片描述会怎样变化呢？很简单，下面给出公式：　　
　在这里插入图片描述
结合方程（4）和（3）得到

1.2 外部控制量

我们并没有捕捉到一切信息，可能存在外部因素会对系统进行控制，带来一些与系统自身状态没有相关性的改变。

以火车的运动状态模型为例，火车司机可能会操纵油门，让火车加速。相同地，在我们机器人这个例子中，导航软件可能会发出一个指令让轮子转向或者停止。如果知道这些额外的信息，我们可以用一个向量Uk来表示，将它加到我们的预测方程中做修正。

假设由于油门的设置或控制命令，我们知道了期望的加速度a，根据基本的运动学方程可以得到：
在这里插入图片描述
以矩阵的形式表示就是：

$B_k$ 称为控制矩阵， $U_k$ 称为控制向量（对于没有外部控制的简单系统来说，这部分可以忽略）。让我们再思考一下，如果我们的预测并不是100%准确的，该怎么办呢？

1.3 外部干扰

如果这些状态量是基于系统自身的属性或者已知的外部控制作用来变化的，则不会出现什么问题。

但是，如果存在未知的干扰呢？例如，假设我们跟踪一个四旋翼飞行器，它可能会受到风的干扰，如果我们跟踪一个轮式机器人，轮子可能会打滑，或者路面上的小坡会让它减速。这样的话我们就不能继续对这些状态进行跟踪，如果没有把这些外部干扰考虑在内，我们的预测就会出现偏差。

在每次预测之后，我们可以添加一些新的不确定性来建立这种与“外界”（即我们没有跟踪的干扰）之间的不确定性模型：

原始估计中的每个状态变量更新到新的状态后，仍然服从高斯分布。我们可以说Xk-1的每个状态变量移动到了一个新的服从高斯分布的区域，协方差为Qk。换句话说就是，我们将这些没有被跟踪的干扰当作协方差为Qk的噪声来处理。

这产生了具有不同协方差（但是具有相同的均值）的新的高斯分布。

我们通过简单地添加Qk得到扩展的协方差，下面给出预测步骤的完整表达式: 在这里插入图片描述
由上式可知，新的最优估计是根据上一最优估计预测得到的，并加上已知外部控制量的修正。　　而新的不确定性由上一不确定性预测得到，并加上外部环境的干扰。

好了，我们对系统可能的动向有了一个模糊的估计，用 $x_k$ 和 $P_k$ 来表示。如果再结合传感器的数据会怎样呢？

1.4 用测量值来修正估计值

我们可能会有多个传感器来测量系统当前的状态，哪个传感器具体测量的是哪个状态变量并不重要，也许一个是测量位置，一个是测量速度，每个传感器间接地告诉了我们一些状态信息。

注意，传感器读取的数据的单位和尺度有可能与我们要跟踪的状态的单位和尺度不一样，我们用矩阵Hk来表示传感器的数据。

我们可以计算出传感器读数的分布，用之前的表示方法如下式所示
在这里插入图片描述
卡尔曼滤波的一大优点就是能处理传感器噪声，换句话说，我们的传感器或多或少都有点不可靠，并且原始估计中的每个状态可以和一定范围内的传感器读数对应起来

从测量到的传感器数据中，我们大致能猜到系统当前处于什么状态。但是由于存在不确定性，某些状态可能比我们得到的读数更接近真实状态。

我们将这种不确定性（例如：传感器噪声）用协方差Rk 表示，该分布的均值就是我们读取到的传感器数据，称之为 $z_k$ 。

现在我们有了两个高斯分布，一个是在预测值附近，一个是在传感器读数附近。

我们必须在预测值和传感器测量值之间找到最优解

那么，我们最有可能的状态是什么呢？对于任何可能的读数(z1,z2)，有两种情况：（1）传感器的测量值；（2）由前一状态得到的预测值。如果我们想知道这两种情况都可能发生的概率，将这两个高斯分布相乘就可以了。在这里插入图片描述
剩下的就是重叠部分了，这个重叠部分的均值就是两个估计最可能的值，也就是给定的所有信息中的最优估计。

把两个具有不同均值和方差的高斯分布相乘，你会得到一个新的具有独立均值和方差的高斯分布！下面用公式讲解

1.4.1 融合高斯分布

一维高斯分布来分析比较简单点
在这里插入图片描述
两个服从高斯分布的函数相乘

重新归一化，使总概率为1，可以得到：

将式（11）中的两个式子相同的部分用 k 表示
下面进一步将式（12）和（13）写成矩阵的形式，如果 Σ 表示高斯分布的协方差，u 表示每个维度的均值，则：
在这里插入图片描述

矩阵K称为卡尔曼增益

1.4.2 整合公式

预测部分：
在这里插入图片描述
测量部分：

将它们放到式（15）中算出它们之间的重叠部分

由式（14）可得卡尔曼增益为：

将式（16）和式（17）得：

$x_k'$ 就是新的最优估计，我们可以将它和 $P_k'$ 放到下一个预测和更新方程中不断迭代。
在这里插入图片描述

2. 代码

当方差很小时，分布具有更高的确定性,如果我们用无人驾驶汽车追踪另一輛汽车，应该选择那一个高斯分布？
应选择方差小的，确定性高

from math import *
def f(mu, sigma2, x):
    return 1/sqrt(2.*pi*sigma2) * exp(-.5*(x-mu)**2 / sigma2)

New Mean and Variance

def update(mean1,var1,mean2,var2):
    new_mean=(var2*mean1+var1*mean2)/(var1+var2)
    new_var=1/(1/var1+1/var2)
    return[new_mean,new_var]

Predict Function
卡尔曼过滤器，即使没有测量物体的速度，也可以通过物体的位置变化，得到物体的速度，进而预测追踪的物体以该速度出现的下一个位置。

def predict(mean1,var1,mean2,var2):
    new_mean=mean1+mean2
    new_var=var1+var2
    return [new_mean,new_var]

measurements=[5.,6.,7.,9.,10.]
motion=[1.,1.,2.,1.,1.]
measurement_sig=4.
motion_sig=2.
mu=0.
sig=10000.

#Please print out ONLY the final values of the mean
#and the variance in a list [mu, sig]. 

# Insert code here
for n in range(len(measurements)):
    [mu,sig]=update(mu,sig,measurements[n],measurement_sig)
    print ('update:',[mu,sig])
    [mu,sig]=predict(mu,sig,motion[n],motion_sig)
    print ('predict:',[mu,sig])

print( [mu, sig])

#update: [4.998000799680128, 3.9984006397441023]
# predict: [5.998000799680128, 5.998400639744102]
# update: [5.999200191953932, 2.399744061425258]
# predict: [6.999200191953932, 4.399744061425258]
# update: [6.999619127420922, 2.0951800575117594]
# predict: [8.999619127420921, 4.09518005751176]
# update: [8.999811802788143, 2.0235152416216957]
# predict: [9.999811802788143, 4.023515241621696]
# update: [9.999906177177365, 2.0058615808441944]
# predict: [10.999906177177365, 4.005861580844194]
# [10.999906177177365, 4.005861580844194]

这段代码部署了整个卡尔曼滤波器，它检查了所有测量元素并默认测量值的个数为运动的n次幂，它使用update 递归公式更新mu和sigma. 如果我们导入第n个测量值和测量不确定性，他会对运动进行同样的操作，这里的预测部分，它使用第n个运动和运动不确定性递归地更新mu和sigma 并把这些全部打印出来。

在跟踪应用程序中，卡尔曼滤波器最令人惊奇的功能之一是即使他没有直接测量，他也能得出对象的速度

然后根据速度预测出该速度出现的未来位置。这就是卡尔曼滤波器在人工智能和控制理论方面成为流行的原因。

Kalman Filter Land

多变量高斯分布
比如，当机器人的位置有多个维度，每一个维度都是高斯分布时，这个位置变量即服从多维高斯分布。不用去关注它的概率分布公式是啥，没用。它的均值是一个向量，方差为协方差矩阵，反应各维度两两之间的（线性）相关性。举个例子，当我们得知二维高斯变量的两个变量的相关系数为正，那么当x大于均值时，它的y也很可能大于均值，在概率分布上表现为右上方倾斜的等高线图

More Kalman Filters
对车辆状态（state）的估计：1.location（observation） 2.velocity（hidden）。我们假设，当前的速度等于上一次的速度，由之前的分布，得出当前可能的概率分布。再通过当前观测到的位置信息推断出隐藏的速度信息。以此方式，不断更新位置概率分布，这很关键！通过假设和一系列的位置，可以不断地推断出速度信息~~~（这是一个Bayes Rule)

Kalman Filter Design

状态转换函数:表示从当前状态到后一个状态的过程
Nx1 的新状态向量 = nxn 状态转换矩阵 * nx1 的当前状态向量
nxn 状态转换矩阵被称为 F （在后续代码中）
测量函数
Z = 1xn 矩阵 * nx1状态向量
这里的 1xn矩阵被称为 H（在后续代码中）

卡尔曼滤波器的实际更新方程
下面讲一些代码中会出现的相关变量名的含义：

x 估计值
P 不确定性协方差矩阵
F 状态转换矩阵
u 移动向量
Z 测量值
H 测量方程
R 测量噪声
K 卡尔曼增益
I 单位矩阵

预测(prediction)过程：

x’ = F*x + u               预测新的x值

p’ = F*P*FT                预测新的P值，其中FT表示F的转置矩阵

测量更新过程（measurementupdate）：

y = Z – H*x                  y表示误差（error）

S = H*P*HT + R               y被映射到S中

K = P*HT*S-1                 K通常被称为卡尔曼增益，S-1 是S的反转矩阵

x’ = x + (K * y)

p’ = (I – K*H)*P

一维空间的运动估计，其中state包含位置和速度，是一个二维变量。

# Write a function 'kalman_filter' that implements a multi-
# dimensional Kalman Filter for the example given
 
from math import *
 
# 定义，实现矩阵的各种操作
class matrix:
    
    # implements basic operations of a matrix class
    
    def __init__(self, value):
        self.value = value
        self.dimx = len(value)
        self.dimy = len(value[0])
        if value == [[]]:
            # (1, 0)
            self.dimx = 0
    
    def zero(self, dimx, dimy):
        # check if valid dimensions
        if dimx < 1 or dimy < 1:
            raise ValueError("Invalid sizeof matrix")
        else:
            self.dimx = dimx
            self.dimy = dimy
            self.value = [[0 for row in range(dimy)] for col in range(dimx)]
    
    def identity(self, dim):
        # check if valid dimension
        if dim < 1:
            raise ValueError("Invalid sizeof matrix")
        else:
            self.dimx = dim
            self.dimy = dim
            self.value = [[0 for row in range(dim)] for col in range(dim)]
            for i in range(dim):
                self.value[i][i] = 1
    
    def show(self):
        for i in range(self.dimx):
            print(self.value[i])
        print(' ')
    
    def __add__(self, other):
        # check if correct dimensions
        if self.dimx != other.dimx or self.dimy != other.dimy:
            raise ValueError("Matrices must be of equal dimensions to add")
        else:
            # add if correct dimensions
            res = matrix([[]])
            res.zero(self.dimx, self.dimy)
            for i in range(self.dimx):
                for j in range(self.dimy):
                    res.value[i][j] = self.value[i][j] + other.value[i][j]
            return res
    
    def __sub__(self, other):
        # check if correct dimensions
        if self.dimx != other.dimx or self.dimy != other.dimy:
            raise ValueError("Matrices must be of equal dimensions to subtract")
        else:
            # subtract if correct dimensions
            res = matrix([[]])
            res.zero(self.dimx, self.dimy)
            for i in range(self.dimx):
                for j in range(self.dimy):
                    res.value[i][j] = self.value[i][j] - other.value[i][j]
            return res
    
    def __mul__(self, other):
        # check if correct dimensions
        if self.dimy != other.dimx:
            raise ValueError("Matrices must be m*n and n*p to multiply")
        else:
            # multiply if correct dimensions
            res = matrix([[]])
            res.zero(self.dimx, other.dimy)
            for i in range(self.dimx):
                for j in range(other.dimy):
                    for k in range(self.dimy):
                        res.value[i][j] += self.value[i][k] * other.value[k][j]
            return res
    
    def transpose(self):
        # compute transpose
        res = matrix([[]])
        res.zero(self.dimy, self.dimx)
        for i in range(self.dimx):
            for j in range(self.dimy):
                res.value[j][i] = self.value[i][j]
        return res
    
    # Thanks to Ernesto P. Adorio for use of Cholesky and CholeskyInverse functions
    
    def Cholesky(self, ztol=1.0e-5):
        # Computes the upper triangular Cholesky factorization of
        # a positive definite matrix.
        res = matrix([[]])
        res.zero(self.dimx, self.dimx)
        
        for i in range(self.dimx):
            S = sum([(res.value[k][i])**2 for k in range(i)])
            d = self.value[i][i] - S
            if abs(d) < ztol:
                res.value[i][i] = 0.0
            else:
                if d < 0.0:
                    raise ValueError("Matrix not positive-definite")
                res.value[i][i] = sqrt(d)
            for j in range(i+1, self.dimx):
                S = sum([res.value[k][i] * res.value[k][j] for k in range(self.dimx)])
                if abs(S) < ztol:
                    S = 0.0
                res.value[i][j] = (self.value[i][j] - S)/res.value[i][i]
        return res
    
    def CholeskyInverse(self):
        # Computes inverse of matrix given its Cholesky upper Triangular
        # decomposition of matrix.
        res = matrix([[]])
        res.zero(self.dimx, self.dimx)
        
        # Backward step for inverse.
        for j in reversed(range(self.dimx)):
            tjj = self.value[j][j]
            S = sum([self.value[j][k]*res.value[j][k] for k in range(j+1, self.dimx)])
            res.value[j][j] = 1.0/tjj**2 - S/tjj
            for i in reversed(range(j)):
                res.value[j][i] = res.value[i][j] = -sum([self.value[i][k]*res.value[k][j] for k in range(i+1, self.dimx)])/self.value[i][i]
        return res
    
    def inverse(self):
        aux = self.Cholesky()
        res = aux.CholeskyInverse()
        return res
    
    def __repr__(self):
        return repr(self.value)
 

 
# Implement the filter function below
# 用均值x 和 协方差P 描述分布的情况，迭代就是不断地更新这两个量
 
def kalman_filter(x, P):
    for n in range(len(measurements)):
        
        # measurement update
        Z = matrix([[measurements[n]]])
        y = Z - (H * x)  # error
        S = H * P * H.transpose() + R
        K = P * H.transpose() * S.inverse()
        
        x = x + (K * y)
        P = (I - (K * H)) * P
        
        # prediction
        x = F * x + u
        P = F * P * F.transpose()
        
        print('x= ')
        x.show()
        print ('P= ')
        P.show()
        
    return x,P
 

# use the code below to test your filter!

 
measurements = [1, 2, 3]  # Z
 
x = matrix([[0.], [0.]]) # initial state (location and velocity)            # estimate
P = matrix([[1000., 0.], [0., 1000.]]) # initial uncertainty 协方差矩阵     # uncertainty convariance
# 计算下一时刻 状态state 的prediction需要的变量
u = matrix([[0.], [0.]]) # external motion                # motion vector
F = matrix([[1., 1.], [0, 1.]]) # next state function     # state transition matrix
# 计算 measurement update需要的变量
H = matrix([[1., 0.]])                                    # measurement function 取x轴坐标值
R = matrix([[1.]]) # measurement uncertainty              # measurement noise方差
 
I = matrix([[1., 0.], [0., 1.]]) # identity matrix
 
print(kalman_filter(x, P))

x= 
# [0.9990009990009988]
# [0.0]
 
# P= 
# [1000.9990009990012, 1000.0]
# [1000.0, 1000.0]
 
# x= 
# [2.998002993017953]
# [0.9990019950129659]
 
# P= 
# [4.990024935169789, 2.9930179531228447]
# [2.9930179531228305, 1.9950129660888933]
 
# x= 
# [3.9996664447958645]
# [0.9999998335552873]
 
# P= 
# [2.3318904241194827, 0.9991676099921091]
# [0.9991676099921067, 0.49950058263974184]
 
# ([[3.9996664447958645], [0.9999998335552873]], [[2.3318904241194827, 0.9991676099921091], [0.9991676099921067, 0.49950058263974184]])

总结

以上的情况是一维空间，在Google自动驾驶车中，用的是二维空间，有x,y的位置及其速度，相应的，状态转移矩阵也就变成4*4了。
【注：感觉Kalman filter的精华在于引入了速度，对物体state进行了预先prediction，再结合实际measure到的位置，确定state，在这个过程中，间接求得了速度】

作业

扩展到2D的程序，很小的改动。

print("### 4-dimensional example ###")
 
 
def filter(x, P):
    for n in range(len(measurements)):
        # 注意 这里改变了顺序，先预测，再measurement
        # prediction
        x = (F * x) + u
        P = F * P * F.transpose()
        
        # measurement update
        Z = matrix([measurements[n]])
        y = Z.transpose() - (H * x)
        S = H * P * H.transpose() + R
        K = P * H.transpose() * S.inverse()
        x = x + (K * y)
        P = (I - (K * H)) * P
    
    print('x= ') 
    x.show()
    print('P= ')
    P.show()
 
measurements = [[5., 10.], 
                [6., 8.], 
                [7., 6.], 
                [8., 4.], 
                [9., 2.], 
                [10., 0.]]
initial_xy = [4., 12.]
 
# measurements = [[1., 4.], 
#                 [6., 0.], 
#                 [11., -4.], 
#                 [16., -8.]]
# initial_xy = [-4., 8.]
 
# measurements = [[1., 17.], 
#                 [1., 15.], 
#                 [1., 13.], 
#                 [1., 11.]]
# initial_xy = [1., 19.]
 
dt = 0.1
 
x = matrix([[initial_xy[0]], [initial_xy[1]], [0.], [0.]]) # initial state (location and velocity)
u = matrix([[0.], [0.], [0.], [0.]]) # external motion
 
#### fill this in, remember to use the matrix() function!: ####
 
P =  matrix([[0.,0.,0.,0.],
             [0.,0.,0.,0.],
             [0.,0.,1000.,0.],
             [0.,0.,0.,1000.]]) # initial uncertainty: 0 for positions x and y, 1000 for the two velocities
F =  matrix([[1.,0.,dt,0.],
             [0.,1.,0.,dt],
             [0.,0.,1.,0.],
             [0.,0.,0.,1.]])   # next state function: generalize the 2d version to 4d
H =  matrix([[1.,0.,0.,0.],
             [0.,1.,0.,0.]])   # measurement function: reflect the fact that we observe x and y but not the two velocities
R =  matrix([[0.1,0.],
             [0.,0.1]])        # measurement uncertainty: use 2x2 matrix with 0.1 as main diagonal
I =  matrix([[1.,0.,0.,0.],
             [0.,1.,0.,0.],
             [0.,0.,1.,0.],
             [0.,0.,0.,1.]])   # 4d identity matrix
 
###### DO NOT MODIFY ANYTHING HERE #######
 
filter(x, P)

输出：

### 4-dimensional example ###
x= 
[9.999340731787717]
[0.001318536424568617]
[9.998901219646193]
[-19.997802439292386]
 
P= 
[0.03955609273706198, 0.0, 0.06592682122843721, 0.0]
[0.0, 0.03955609273706198, 0.0, 0.06592682122843721]
[0.06592682122843718, 0.0, 0.10987803538073201, 0.0]
[0.0, 0.06592682122843718, 0.0, 0.10987803538073201]

关于motion vecter u 的含义：有可能车辆不是匀速运动，而是有一个加速度。

关于measurement noise R 的含义：对位置测量的准确性。