无人车技术基础——课程2：Kalman Filters

1）简介

Stanford's Research Center：最新版的无人车，设备介绍：laser-range finder（激光测距仪），每秒进行10次distance scan，返回近一百万个数据点，这些是Kalman filter 的 input。车顶有相机，结合其他的相机组成 stereo camera system。车尾有GPS的天线，接受GPS信息辅助定位。车辆行驶时，不仅要知道附近车辆的位置，还要知道他们的速度，移动方向。这样才能保证车辆安全行驶。

之前用概率定位的方法叫做 Monte Carlo Localization，multimodel distribution，discrete。

Kalman Filter，unimodel distribution，continuous，这两种方法都可用于机器人定位和车辆跟踪。

之后的课程还会讲particle filters，他是multimodel distribution，continuous。

Monte Carlo根据观测到的数据，估计未来的位置和速度。

Kalman filter中的连续分布是高斯分布，分布越集中，定位的确定性越高。

measurement update 过程：

先验分布比较spread，再一次measurement比较peaky，请问后验概率分布的样子。The new belief will be more certain than either the previous belief OR the measurement.

The takeaway lesson here: more measurements means greater certainty. 峰值比两个component都要高，即方差比两个组成分布的方差都小。

有公式：均值为（交叉）权重和，方差为倒数和的倒数。

motion update（predict） 过程：

移动距离也服从一个高斯分布，最终结果：均值为两者均值的和，方差为两者方差的和。

#For this problem, you aren't writing any code.
#Instead, please just change the last argument 
#in f() to maximize the output.

from math import *

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

print f(10.,4.,10.) #Change the 8. to something else!

# Write a program that will iteratively update and
# predict based on the location measurements 
# and inferred motions shown below.
# 这就是一维 Kalman Filter 的实施过程

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

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 = 0.0000000001 # 10000. 通过这两个对比可以发现，初始的wrong estimate 对整个定位过程产生了影响

for i in range(len(measurements)):
    [mu, sig] = update(mu, sig, measurements[i], measurement_sig)
    print 'update: ',[mu, sig]
    [mu, sig] = predict(mu,sig, motion[i], motion_sig)
    print 'update: ',[mu, sig]
    
print [mu, sig]

Kalman Filters 可以figure out 物体运动的速度（尽管没有直接measure），从而预测未来的位置。这很棒，也是他很流行的原因。

2）Kalman Filter Land

多变量高斯分布

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

3）More Kalman Filters

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

4）Kalman Filter Design

一维空间的运动估计，其中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 size of 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 size of 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))
# output should be:
# x: [[3.9996664447958645], [0.9999998335552873]]
# P: [[2.3318904241194827, 0.9991676099921091], [0.9991676099921067, 0.49950058263974184]]

5）总结

以上的情况是一维空间，在Google自动驾驶车中，用的是二维空间，有x,y的位置及其速度，相应的，状态转移矩阵也就变成4*4了。

【注：感觉Kalman filter的精华在于引入了速度，对物体state进行了预先prediction，再结合实际measure到的位置，确定state，在这个过程中，间接求得了速度】

6）作业

扩展到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)

7）答疑

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

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