无人驾驶2：卡尔曼滤波行人状态估计

野性酷女 2023-07-04 06:09 102阅读 0赞

案例场景

传感器能够直接观测到某行人的速度Vx,Vy，用卡尔曼滤波估算该行人的状态(包含速度和位置）

相关变量约定如下：

行人状态 x = ( p x , p y , v x , v y ) T x = (p_x,p_y,v_x, v_y)^T x=(px​,py​,vx​,vy​)T

P: 行人不确定性，协方差矩阵

F: 状态转移矩阵

Q: 过程噪声协方差矩阵

K: 卡尔曼滤波增益

H: 观测矩阵

R: 观测噪声协方差矩阵

step0:随机产生一批测量数据，包含二维速度测量值

  1. import numpy as np
  2. %matplotlib inline 
  3. import matplotlib.pyplot as plt
  4. from scipy.stats import norm
  6. m = 200 #measurements
  7. vx = 20
  8. vy = 10
  10. mx = np.array(vx + np.random.randn(m))
  11. my = np.array(vy + np.random.randn(m))
  12. measurements = np.vstack((mx,my))
  13. print(measurements.shape)
  14. print('Standard Deviation of Acceleration Measurements=%0.2f'%np.std(mx))
  15. print('You assumed %0.2f in R.'%R[0,0])

执行结果：
(2, 200)
Standard Deviation of Acceleration Measurements=1.04
You assumed 0.09 in R.

  1. fig = plt.figure(figsize=(16,5))
  2. plt.step(range(m), mx, label='$\dot x $')
  3. plt.step(range(m), my, label='$\dot y $')
  4. plt.ylabel(r'Velocity $m/s$')
  5. plt.title('Measurements')
  6. plt.legend(loc='best',prop={ 'size':18})
  7. plt.savefig('measurements.png')

在这里插入图片描述

step1:初始化行人状态

包括x,y方向的位置和速度，及行人的不确定性，测量间隔时间dt

x: 行人状态初始值, 因未知，所以全设置为0

P: 行人不确定性初始值，不确定性很高

  1. x = np.matrix([[0.0,0.0,0.0,0.0]]).T
  2. print(x,x.shape)
  3. P=np.diag([1000.0,1000.0,1000.0,1000.0])
  4. print(P,P.shape)

执行结果：
[[0.]
[0.]
[0.]
[0.]] (4, 1)
[[1000. 0. 0. 0.]
[ 0. 1000. 0. 0.]
[ 0. 0. 1000. 0.]
[ 0. 0. 0. 1000.]] (4, 4)

step2:设计过程模型和过程噪声协方差矩阵

设计卡尔曼滤波器时，必须定义两个线性函数，如下图：

ekl006.png

状态转移函数F：该函数对从时间k-1到时间k的状态变换进行建模;

测量函数H： 该函数对测量值的计算方式，以及测量值和预测值状态x的关联进行建模;

这些函数的第一部分F，H是模型中的确定性部分，尾项噪声v和噪声w表述的时随机部分，影响预测和测量更新步骤的随机误差；

(1)假设运动为恒速模型，即行人速度不变，过程模型可以描述如下

ekl016.png
即运动模型为：
x k + 1 = [ 1.0 0. d t 0. 1.0 0. 0. d t 0 0 1 0 0 0 0 1 ] ∗ [ p x p y v x v y ] x_k+1 = \left[\begin{matrix} 1.0&0.&dt&0.\\ 1.0&0.&0.&dt\\ 0&0&1&0 \\ 0&0&0&1 \\ \end{matrix} \right]*\left[\begin{matrix} p_x\\ p_y\\ v_x \\ v_y \\ \end{matrix} \right] xk​+1=⎣⎢⎢⎡​1.01.000​0.0.00​dt0.10​0.dt01​⎦⎥⎥⎤​∗⎣⎢⎢⎡​px​py​vx​vy​​⎦⎥⎥⎤​
(2)实际上行人运动过程不一定为恒速，总会有内因(行人自己加速度，无人车内部的加速度控制)和外因(风速，路面光滑程度)等影响；

内因用u表示，是行人或无人车内部控制向量，B是输入控制矩阵；

Bu表示行人由于自身内部动力，引起状态变化；

v表示由于风速，路滑等外因引起的状态变化量，是个随机变量，称为过程噪声；

对一个行人连续观察两次，获得初始速度和最终速度，根据根据动力学公式，可以推导出当前时刻状态和上一时刻状态的函数关系，
在这里插入图片描述
由于观测物体(行人)自身加速度无法准确和预估，应用中常设置Bu=0, 就用随机变量v(均值为0的高斯噪声)作为随机噪声；
在这里插入图片描述
由于加速度未知，可以把加速度加到误差分量中，添加v噪声（加速度和过程噪声都用v来表征）后的过程模型为：
在这里插入图片描述
矩阵表示为：
x k + 1 = [ 1.0 0. d t 0. 1.0 0. 0. d t 0 0 1 0 0 0 0 1 ] ∗ [ p x p y v x v y ] + [ 1 2 a x d t 2 1 2 a y d t 2 a x d t a y d t ] x_k+1 = \left[\begin{matrix} 1.0&0.&dt&0.\\ 1.0&0.&0.&dt\\ 0&0&1&0 \\ 0&0&0&1 \\ \end{matrix} \right]*\left[\begin{matrix} p_x\\ p_y\\ v_x \\ v_y \\ \end{matrix} \right] + \left[\begin{matrix} \frac{1}{2}a_xdt^2\\ \frac{1}{2}a_ydt^2\\ a_xdt \\ a_ydt \\ \end{matrix} \right] xk​+1=⎣⎢⎢⎡​1.01.000​0.0.00​dt0.10​0.dt01​⎦⎥⎥⎤​∗⎣⎢⎢⎡​px​py​vx​vy​​⎦⎥⎥⎤​+⎣⎢⎢⎡​21​ax​dt221​ay​dt2ax​dtay​dt​⎦⎥⎥⎤​

将v分解为两个分量：一个4x2的矩阵G(不包含随机变量)和一个2x1的矩阵a(包含随机加速度分量)。

根据误差向量，现在可以定义新的协方差矩阵Q：协方差矩阵定位为误差向量的期望;

因为G不包含随机变量，可以放在期望外面
在这里插入图片描述
假 定 a x 和 a y 是 无 关 连 的 噪 声 ， 那 么 σ z x y = 0 假定a_x和a_y是无关连的噪声，那么\sigma_{zxy}=0 假定ax​和ay​是无关连的噪声，那么σzxy​=0
在这里插入图片描述
合并回原矩阵得到如下过程协方差矩阵
在这里插入图片描述
噪声Q本质上是一个均值为0的高斯分布v~N(0,Q), 卡尔曼公式2就变成：

p k ′ = a p k a T + Q p_k^{‘} = ap_ka^T + Q pk′​=apk​aT+Q

转移矩阵表示为：
F = [ 1.0 0. d t 0. 1.0 0. 0. d t 0 0 1 0 0 0 0 1 ] F= \left[\begin{matrix} 1.0&0.&dt&0.\\ 1.0&0.&0.&dt\\ 0&0&1&0 \\ 0&0&0&1 \\ \end{matrix} \right] F=⎣⎢⎢⎡​1.01.000​0.0.00​dt0.10​0.dt01​⎦⎥⎥⎤​

  1. dt = 0.1 # Time step between Filters steps
  2. F = np.matrix([[1.0,0.0,dt,0.0],
  3.                [0.0,1.0,0.0,dt],
  4.                [0.0,0.0,1.0,0.0],
  5.                [0.0,0.0,0.0,1.0]])
  6. print(F,F.shape)

执行结果：
[[1. 0. 0.1 0. ]
[0. 1. 0. 0.1]
[0. 0. 1. 0. ]
[0. 0. 0. 1. ]] (4, 4)

  1. ‘’‘
  2. sv = 0.5
  3. G = np.matrix([[0.5*dt**2],
  4.               [0.5*dt**2],
  5.               [dt],
  6.               [dt]])
  7. Q = G*G.T*sv*2
  8. from sympy import Symbol, Matrix
  9. from sympy.interactive import printing
  10. printing.init_printing()
  11. dts = Symbol('dt')
  12. ’‘’
  14. noise_ax=0.5
  15. noise_ay=0.5
  16. dt_2 = dt*dt;
  17. dt_3 = dt_2 *dt;
  18. dt_4 = dt_3*dt;
  20. Q = np.matrix([[0.25*dt_4*noise_ax,0,0.5*dt_3*noise_ax,0],
  21.                [0, 0.25*dt_4*noise_ay,0, 0.25*dt_3*noise_ay],
  22.                [dt_3/2*noise_ax, 0, dt_2*noise_ax, 0],
  23.                [0, dt_3/2*noise_ay, 0, dt_2*noise_ay]])

执行结果：
[[0.09 0. ]
[0. 0.09]] (2, 2)

step3: 设计测量模型，观测噪声

(1)使用传感器可以直接测量行人的速度Vx, Vy，
Z = [ v x v y ] Z= \left[\begin{matrix} v_x\\ v_y\\ \end{matrix} \right] Z=[vx​vy​​]
(2) 测量矩阵可以表示为：
H = [ 0 0 1 0 0 0 0 1 ] H= \left[\begin{matrix} 0&0&1&0\\ 0&0&0&1 \\ \end{matrix} \right] H=[00​00​10​01​]

(3)测量噪声的协方差矩阵为：

R = [ σ v x 2 0 0 σ v y 2 ] R= \left[\begin{matrix} \sigma_{v_x}^2&0\\ 0&\sigma_{v_y}^2 \\ \end{matrix} \right] R=[σvx​2​0​0σvy​2​​]

σ v x 2 , σ v y 2 \sigma_{v_x}^2, \sigma_{v_y}^2 σvx​2​,σvy​2​描述了传感器的测量有“多差”，是传感器固有性质，一般有厂商提供

  1. H = np.matrix([[0.0,0.0,1.0,0.0],
  2.                [0.0,0.0,0.0,1.0]])
  3. print(H, H.shape)
  4. ra = 0.09 #厂商提供
  5. R = np.matrix([[ra,0.0],
  6.               [0.0,ra]])
  7. print(R,R.shape)

执行结果：
[[0. 0. 1. 0.]
[0. 0. 0. 1.]] (2, 4)
[[0.09 0. ]
[0. 0.09]] (2, 2)

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

执行结果：
[[1. 0. 0. 0.]
[0. 1. 0. 0.]
[0. 0. 1. 0.]
[0. 0. 0. 1.]] (4, 4)

一些过程值，用于结果显示

  1. xt = []
  2. yt = []
  3. dxt = []
  4. dyt = []
  6. Zx = []
  7. Zy = []
  9. Px = []
  10. Py = []
  11. Pdx = []
  12. Pdy = []
  14. Rdx = []
  15. Rdy = []
  17. Kx = []
  18. Ky = []
  19. Kdx =[]
  20. Kdy = []
  22. def save_states(x,Z,P,R,K):
  23.     xt.append(float(x[0]))
  24.     yt.append(float(x[1]))
  25.     dxt.append(float(x[2]))
  26.     dyt.append(float(x[3]))
  28.     Zx.append(float(Z[0]))
  29.     Zy.append(float(Z[1]))
  31.     Px.append(float(P[0,0]))
  32.     Py.append(float(P[1,1]))
  33.     Pdx.append(float(P[2,2]))
  34.     Pdy.append(float(P[3,3]))
  36.     Rdx.append(float(R[0,0]))
  37.     Rdy.append(float(R[1,1]))
  39.     Kx.append(float(K[0,0]))
  40.     Ky.append(float(K[1,0]))
  41.     Kdx.append(float(K[2,0]))
  42.     Kdy.append(float(K[3,0]))

step4: 卡尔曼公式

008.png

  1. for n in range(len(measurements[0])):
  2.     #Time Update(Prediction)
  3.     # ==============================
  4.     x = F*x             #Project the state ahead
  5.     P = F * P *F.T + Q  #Project the error covariance ahead
  7.     # Measurement Update (Correction)
  8.     #==============================
  9.     S = H*P*H.T + R
  10.     K = (P*H.T)*np.linalg.pinv(S)
  12.     #Update the estimate via z
  13.     Z = measurements[:,n].reshape(2,1)
  14.     y = Z - (H*x)
  15.     x = x + (K*y)
  17.     #update the error convariance
  18.     P = (I - (K*H))*P
  20.     #save states (for Plotting)
  21.     save_states(x,Z,P,R,K)
  23. def plot_x():
  24.     fig = plt.figure(figsize=(16,9))
  25.     plt.step(range(len(measurements[0])), dxt, label='$estimateVx $')
  26.     plt.step(range(len(measurements[0])), dyt, label='$estimateVy $')
  28.     plt.step(range(len(measurements[0])),measurements[0],label='$measurementVx$')
  29.     plt.step(range(len(measurements[0])),measurements[1],label='$measurementVy$')
  31.     #plt.axhline(vx, colors='#999999',label = '$trueVx$')
  32.     #plt.axhline(vy, colors='#999999',label = '$trueVy$')
  34.     plt.xlabel('Filter Step')
  35.     plt.title('Estimate (Elements from State Vector $x$)')
  36.     plt.legend(loc='best',prop={ 'size':11})
  37.     plt.ylim([0,30])
  38.     plt.ylabel('Velocity')
  39. plot_x()

卡尔曼滤波关于速度的估计结果
在这里插入图片描述

  1. def plot_xy():
  2.     fig = plt.figure(figsize=(16,9))
  3.     plt.scatter(xt,yt,s=20,label='State', c = 'k')
  4.     plt.scatter(xt[0],yt[0],s = 100, label='Start', c = 'g')
  5.     plt.scatter(xt[-1], yt[-1],s=100,label='Goal', c = 'r')
  7.     plt.xlabel('X')
  8.     plt.ylabel('Y')
  9.     plt.legend('Position')
  10.     plt.axis('equal')
  12. plot_xy()

卡尔曼滤波位置估算结果：
在这里插入图片描述

发表评论

表情：
评论列表 （有 0 条评论，102人围观）

还没有评论，来说两句吧...

相关阅读

    相关 无人驾驶6：马尔卡夫滤波

    无人车定位问题 准确定位，是无人车技术的基础，常用的GPS定位，误差经常为2～10m，而无人车的精度要求2~10cm左右，怎么实现呢，这就是无人车的定位问题。 ![000]

    小咪咪小咪咪/ 0 赞/ 127 阅读

    相关 无人驾驶4： 无损卡尔曼滤波

    在上一节的扩展卡尔曼滤波跟踪系统中，有两个缺陷： 1. 系统采用恒速模型：假定行人沿直线运动；实际应用中，出现曲线运动时，预估不够准确。 2. 每次测量都需要计算雅可比矩

    傷城~傷城~/ 0 赞/ 236 阅读