无人驾驶实战（三）——基于卡尔曼滤波（KF）的行人位置及速度的估算

卡尔曼滤波的背景知识：
　　在物体跟踪或者预测过程中，我们需要对一些感兴趣的目标状态进行状态估计预测，但是测量是会存在误差和噪声的，所以我们对我们的测量结果是不相信的，因此我们需要采用概率学和统计学的方法来分析统计和估计状态量。

卡尔曼滤波的五个过程步骤：
　　卡尔曼滤波是一个递推算法，每次递推分为两步：1、计算出一个预测值；2、对预测值和测量值进行加权求和得到最优估计值。
具体步骤如下：
（1）、根据 k-1 时刻的最优估计值 x^k-1 ，计算 k 时刻的预测值 x’k
（2）、根据 k-1 时刻的最优估计值的误差 pk-1 ，计算 k 时刻的预测值的误差 p’k
（3）、根据 k 时刻的预测值的误差 p’k 和 k 时刻的测量值的误差 r ，计算 k 时刻的卡尔曼增益 kk
（4）、根据 k 时刻的预测值 x’k 、k 时刻的测量值 zk 和 k 时刻的卡尔曼增益 kk ，计算 k 时刻的最优估
　　　　计值 x^k
（5）、根据 k 时刻的预测值的误差 p’k 和 k 时刻的卡尔曼增益 kk ，计算 k 时刻的最优估计值 x^k 的误差 pk

卡尔曼滤波的具体应用公式：
状态预测：
1、预测状态：
　　x’ = F x^ + B u　　　　（F：状态转移矩阵、B u ：控制信号，非真实场景若无法测量，通常取0）
2、预测状态误差：
　　p’ = F p FT + Q 　　　　（Q：预测噪声协方差矩阵，也即高斯分布的不确定性）

测量更新：
1、预测状态与测量值差值
　　y = z - H x’ 　　　　　　　(H：测量矩阵)
2、计算卡尔曼增益所需的中间量
　　s = H p’ HT + R 　　　　　(R：测量噪声协方差矩阵)
3、克尔曼增益
　　k = p’ HT s-1

x^ = x’ + k y
p = (I - k H) p’

卡尔曼滤波的具体应用实例——基于卡尔曼滤波的行人位置估算：
1、行人的状态 x：
　　x = (px , py , vx , vy)T　　　（位置分量与速度分量）
　　
2、过程模型 x’：
　　x’ = F x^ + v
考虑行人运动过程中的噪声（V：加速或者减速）后，展开上述公式：

3、预测噪声协方差矩阵 Q：

4、测量矩阵 H：
　　H = ([ [0 0 1 0] [0 0 0 1] ])T
　　
5、测量噪声协方差矩阵 R：

使用python代码实现

1、导入一些必要的库
代码：

import numpy as np
import matplotlib.pyplot as plt
from scipy.stats import norm
from sympy import Symbol, Matrix

代码讲解：
（1）from scipy.stats import norm：统计函数库 scipy.stats
（2）from sympy import Symbol, Matrix：数学符号库，通俗的讲就是使得符号可以参与数学计算

2、初始化：行人状态 x ，预测状态误差 p ，测量时间间隔 dt ， 状态转移矩阵 F ， 测量矩阵 H ，
　　测量噪声协方差矩阵 R， 预测噪声协方差矩阵 Q，4*4 单位矩阵
代码：

x = np.matrix([[0.0, 0.0, 0.0, 0.0]]).T
print(x, x.shape)

P = np.diag([1000.0, 1000.0, 1000.0, 1000.0])
print(P, P.shape)

dt = 0.1 # Time Step between Filter Steps
F = np.matrix([[1.0, 0.0, dt, 0.0],
              [0.0, 1.0, 0.0, dt],
              [0.0, 0.0, 1.0, 0.0],
              [0.0, 0.0, 0.0, 1.0]])
print(F, F.shape)

H = np.matrix([[0.0, 0.0, 1.0, 0.0],
              [0.0, 0.0, 0.0, 1.0]])
print(H, H.shape)

ra = 0.09
R = np.matrix([[ra, 0.0],
              [0.0, ra]])
print(R, R.shape)

sv = 0.5
G = np.matrix([[0.5*dt**2],
               [0.5*dt**2],
               [dt],
               [dt]])
Q = G*G.T*sv**2

dts = Symbol('dt')
Qs = Matrix([[0.5*dts**2],[0.5*dts**2],[dts],[dts]])
Qs*Qs.T
print(Qs*Qs.T)

I = np.eye(4)
print(I, I.shape)

结果展示：
（1）初始化输出显示

3、产生一些随机数测量数据矩阵
代码：

m = 200 # Measurements
vx= 20 # in X
vy= 10 # in Y

mx = np.array(vx+np.random.randn(m))
my = np.array(vy+np.random.randn(m))
measurements = np.vstack((mx,my))

print(measurements.shape)
print('Standard Deviation of Acceleration Measurements=%.2f' % np.std(mx))
print('You assumed %.2f in R.' % R[0,0])

fig = plt.figure(figsize=(16,5))
plt.step(range(m),mx, label='$\dot x$')
plt.step(range(m),my, label='$\dot y$')
plt.ylabel(r'Velocity $m/s$')
plt.title('Measurements')
plt.legend(loc='best',prop={'size':18})
plt.show()

结果展示：
（1）矩阵的属性

（2）可视化位置与速度的测量数据

4、一些过程值，用于结果的显示
代码：

xt = []
yt = []
dxt= []
dyt= []
Zx = []
Zy = []
Px = []
Py = []
Pdx= []
Pdy= []
Rdx= []
Rdy= []
Kx = []
Ky = []
Kdx= []
Kdy= []

def savestates(x, Z, P, R, K):
    xt.append(float(x[0]))
    yt.append(float(x[1]))
    dxt.append(float(x[2]))
    dyt.append(float(x[3]))
    Zx.append(float(Z[0]))
    Zy.append(float(Z[1]))
    Px.append(float(P[0,0]))
    Py.append(float(P[1,1]))
    Pdx.append(float(P[2,2]))
    Pdy.append(float(P[3,3]))
    Rdx.append(float(R[0,0]))
    Rdy.append(float(R[1,1]))
    Kx.append(float(K[0,0]))
    Ky.append(float(K[1,0]))
    Kdx.append(float(K[2,0]))
    Kdy.append(float(K[3,0]))

5、遍历测量数据，运行卡尔曼滤波器（状态预测、测量更新）
代码：

for n in range(len(measurements[0])):

    # Time Update (Prediction)
    # ========================
    # Project the state ahead
    x = F*x

    # Project the error covariance ahead
    P = F*P*F.T + Q

    # Measurement Update (Correction)
    # ===============================
    # Compute the Kalman Gain
    S = H*P*H.T + R
    K = (P*H.T) * np.linalg.pinv(S)

    # Update the estimate via z
    Z = measurements[:,n].reshape(2,1)
    y = Z - (H*x)                            # Innovation or Residual
    x = x + (K*y)

    # Update the error covariance
    P = (I - (K*H))*P

    # Save states (for Plotting)
    savestates(x, Z, P, R, K)

6、显示关于速度的估计结果
代码：

def plot_x():
    fig = plt.figure(figsize=(16,9))
    plt.step(range(len(measurements[0])),dxt, label='$estimateVx$')
    plt.step(range(len(measurements[0])),dyt, label='$estimateVy$')

    plt.step(range(len(measurements[0])),measurements[0], label='$measurementVx$')
    plt.step(range(len(measurements[0])),measurements[1], label='$measurementVy$')

    plt.axhline(vx, color='#999999', label='$trueVx$')
    plt.axhline(vy, color='#999999', label='$trueVy$')

    plt.xlabel('Filter Step')
    plt.title('Estimate (Elements from State Vector $x$)')
    plt.legend(loc='best',prop={'size':11})
    plt.ylim([0, 30])
    plt.ylabel('Velocity')
    plt.show()
    
plot_x()

结果展示：
（1）不同的颜色分别代表了预测、测量、真实之间的可视化结果

7、显示关于位置的估计结果
代码：

def plot_xy():
    fig = plt.figure(figsize=(16,16))
    plt.scatter(xt,yt, s=20, label='State', c='k')
    plt.scatter(xt[0],yt[0], s=100, label='Start', c='g')
    plt.scatter(xt[-1],yt[-1], s=100, label='Goal', c='r')

    plt.xlabel('X')
    plt.ylabel('Y')
    plt.title('Position')
    plt.legend(loc='best')
    plt.axis('equal')
    plt.show()
    
plot_xy()

结果展示：
（1）位置的变化可视化结果，行人近似为匀速行走

　　至此，最简单的卡尔曼滤波应用实例就讲述完成了，本博客仅仅只讲了卡尔曼滤波的基本形式，并不是实际无人车项目中的所使用的技术，但是其基本原理与我们目前在Google无人车感知模块的技术是相通的。

使用C++代码实现

等待后续更新。。。