源码阅读-PythonRobotics-Localization-EKF

本篇笔记是对PythonRobotics中Localization部分的EKF源码进行翻译和分析，以作学习之用，如有翻译理解不对的地方，烦请大家留言指出，谢谢！

Author: Sunshengjin
Mail: [email protected]

这是此滤波器仿真图像

• 蓝色线为机器人的真实轨迹

• 黑色线为航迹推演的轨迹

• 绿色点表示GPS数据

• 红色线为EKF的推测轨迹

• 红色椭圆为EKF的估计协方差区域

过程分析

滤波器设计

在这个仿真中，机器人的状态由四个向量组成：
$x_t = [ x_t, y_t,\phi_t,v_t]$xt​=[xt​,yt​,ϕt​,vt​]

• $x,y$x,y是2D的x-y位置，$ \phi $是方向，$是方向，v$是速度

• 在代码中，“xEST”代表的是机器人状态矩阵

• $P_t$Pt​是机器人状态协方差矩阵

• $Q$Q是运行噪声协方差矩阵

• $R$R是在 $t$t时刻的观测噪声协方差矩阵

机器人安装了速度传感器和角度传感器，所以，机器人状态在 $t$t时刻的输入矩阵*(input vector)* 可以写成：
$u_t = [v_t,w_t]$ut​=[vt​,wt​]
机器人也安装了GNSS传感器，所以机器人可以观测到自己 $t$t时刻的x-y坐标
$z_t = [x_t,y_t]$zt​=[xt​,yt​]
输入矩阵与观测矩阵都包含了传感器噪声，在代码中，“观测”部分的函数生成了带有噪声的输入和观测矩阵：

def observation(xTrue, xd, u):
     xTrue = motion_model(xTrue, u)
     # add noise to gps x-y
     zx = xTrue[0, 0] + np.random.randn() * Qsim[0, 0]
     zy = xTrue[1, 0] + np.random.randn() * Qsim[1, 1]
     z = np.array([[zx, zy]])
     # add noise to input
     ud1 = u[0, 0] + np.random.randn() * Rsim[0, 0]
     ud2 = u[1, 0] + np.random.randn() * Rsim[1, 1]
     ud = np.array([[ud1, ud2]]).T
     xd = motion_model(xd, ud)
     return xTrue, z, xd, ud

运动模型

机器人模型为：
$\dot{\phi}=w$ϕ˙​=w
运动模型为：
$x_{t-1}=Fx_t+Bu_t$xt−1​=Fxt​+But​
其中：
$F= \begin{bmatrix} 1 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \\ \end{bmatrix}$F=⎣⎢⎢⎡​1000​0100​0010​0001​⎦⎥⎥⎤​
$B= \begin{bmatrix} \cos(\phi)dt & 0 \\ \sin(\phi)de & 0 \\ 0 & dt \\ 1 & 0 \\ \end{bmatrix}$B=⎣⎢⎢⎡​cos(ϕ)dtsin(ϕ)de01​00dt0​⎦⎥⎥⎤​

• $dt$dt 是时间间隔
  这一部分的代码为：

def motion_model(x, u):
     F = np.array([[1.0, 0, 0, 0],
         [0, 1.0, 0, 0],
         [0, 0, 1.0, 0],
         [0, 0, 0, 0]])
     B = np.array([[DT * math.cos(x[2, 0]), 0],
         [DT * math.sin(x[2, 0]), 0],
         [0.0, DT],
         [1.0, 0.0]])
     x = F.dot(x) + B.dot(u)
     return x

雅克比矩阵为：
$J_F = \begin{bmatrix} \frac{dx}{dx} & \frac{dx}{dy} & \frac{dx}{d\phi} & \frac{dx}{dv} \\ \frac{dy}{dx} & \frac{dy}{dy} & \frac{dy}{d\phi} & \frac{dy}{dv} \\ \frac{d\phi}{dx} & \frac{d\phi}{dy} & \frac{d\phi}{d\phi} & \frac{d\phi}{dv} \\ \frac{dv}{dx} & \frac{dv}{dy} & \frac{dv}{d\phi} & \frac{dv}{dv}\\ \end{bmatrix} =\begin{bmatrix} 1 & 0 & -v\sin(\phi)dt & \cos(\phi)dt \\ 0 & 1 & v\cos(\phi)dt & \sin(\phi)dt \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \\ \end{bmatrix}$JF​=⎣⎢⎢⎢⎡​dxdx​dxdy​dxdϕ​dxdv​​dydx​dydy​dydϕ​dydv​​dϕdx​dϕdy​dϕdϕ​dϕdv​​dvdx​dvdy​dvdϕ​dvdv​​⎦⎥⎥⎥⎤​=⎣⎢⎢⎡​1000​0100​−vsin(ϕ)dtvcos(ϕ)dt10​cos(ϕ)dtsin(ϕ)dt01​⎦⎥⎥⎤​

观测模型

机器人可以从GPS传感器获取自身的x-y位置数据，GPS的观测矩阵为：
$z_t = Hx_t$zt​=Hxt​
其中：
$B = \begin{bmatrix} 1 & 0 & 0 & 0\\ 0 & 1 & 0 & 0\\ \end{bmatrix}$B=[10​01​00​00​]
雅各比矩阵为：
$J_H = \begin{bmatrix} \frac{dx}{dx} & \frac{dx}{dy} & \frac{dx}{d\phi} & \frac{dx}{dv} \\ \frac{dy}{dx} & \frac{dy}{dy} & \frac{dy}{d\phi} & \frac{dy}{dv} \\ \end{bmatrix} = \begin{bmatrix} 1 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 \\ \end{bmatrix}$JH​=[dxdx​dxdy​​dydx​dydy​​dϕdx​dϕdy​​dvdx​dvdy​​]=[10​01​00​00​]

扩展卡尔曼滤波部分

定位方法采用扩展卡尔曼滤波的方法，整个定位流程如下：

---

预测过程

$x_{Pred} = Fx_t + Bu_t$xPred​=Fxt​+But​
$P_{Pred} = J_FP_tJ^T_F+Q$PPred​=JF​Pt​JFT​+Q

---

更新过程

$z_{Pred} = Hx_{Pred}$zPred​=HxPred​
$y = z - z_{Pred}$y=z−zPred​
$S = J_HP_{Pred}J^T_H + R$S=JH​PPred​JHT​+R
$K = P_{Pred}J^T_HS^{-1}$K=PPred​JHT​S−1
$x_{t+1} = x_{Pred}+K_y$xt+1​=xPred​+Ky​
$P_{t+1} = (I - KJ_H)P_{Pred}$Pt+1​=(I−KJH​)PPred​

源码解析

import numpy as np
 import math
 import matplotlib.pyplot as plt

首先导入所需要的py函数库

---

# Estimation parameter of EKF
 Q = np.diag([0.1, 0.1, np.deg2rad(1.0), 1.0])**2 # predict state covariance
 R = np.diag([1.0, 1.0])**2 # Observation x,y position covariance

创建EKF框架中的误差协方差矩阵

---

# Simulation parameter
 Qsim = np.diag([1.0, np.deg2rad(30.0)])**2
 Rsim = np.diag([0.5, 0.5])**2

创建机器人模型中的误差协方差矩阵

---

DT = 0.1 # time tick [s]
 SIM_TIME = 50.0 # simulation time [s]
 show_animation = True

设置时间间隔为 0.1s，仿真时间为50s，并且打开显示动画

---

def calc_input():
 v = 1.0 # [m/s]
 yawrate = 0.1 # [rad/s]
 u = np.array([[v, yawrate]]).T
 return u

仿真输入矩阵，设定了仿真速度，创建了输入矩阵u

---

def observation(xTrue, xd, u):
     xTrue = motion_model(xTrue, u)
     # add noise to gps x-y
     zx = xTrue[0, 0] + np.random.randn() * Rsim[0, 0]
     zy = xTrue[1, 0] + np.random.randn() * Rsim[1, 1]
     z = np.array([[zx, zy]]).T
     # add noise to input
     ud1 = u[0, 0] + np.random.randn() * Qsim[0, 0]
     ud2 = u[1, 0] + np.random.randn() * Qsim[1, 1]
     ud = np.array([[ud1, ud2]]).T
     xd = motion_model(xd, ud)
     return xTrue, z, xd, ud

上文提到过，这段代码仿真了观测模型。
第2行：计算实际值在输入矩阵u影响后的下一帧位置数据
第4-5行：向GPS数据中叠加噪声，模拟实际获取到的GPS数据
第8-9行：向输入矩阵中叠加噪声，模拟实际机器人在受到给定控制后执行动作时所包含的噪声
第11行：模拟机器人在实际包含噪声的控制作用下运行后的下一帧位置数据

输入值：

1.xTrue： 机器人的Groud Truth位置
2.xd： 机器人观测数据
3.u： 控制矩阵

返回值：

1. xTrue : 机器人的Groud Truth位置
2. z ：向Groud Truth中叠加噪声，模拟GPS数据
3. xd ：机器人在有噪声的控制后的观测数据
4. ud ：叠加了噪声后的控制输入，模拟机器人实际运动

---

def motion_model(x, u):
 F = np.array([[1.0, 0, 0, 0],
          [0, 1.0, 0, 0],
         [0, 0, 1.0, 0],
         [0, 0, 0, 0]])
     B = np.array([[DT * math.cos(x[2, 0]), 0],
         [DT * math.sin(x[2, 0]), 0],
         [0.0, DT],
         [1.0, 0.0]])
     x = [email protected] + [email protected]
     return x

机器人运动模型函数（传递函数），计算输入位置矩阵在输入控制矩阵作用后的机器人的位置信息，具体原理见上文

• @ : 矩阵相乘

输入值：

1. x ：机器人位置
2. u ：控制信息

输出值：

1. x ：在给定控制输入后，机器人的位置信息

---

def observation_model(x):
     # Observation Model
     H = np.array([
         [1, 0, 0, 0],
         [0, 1, 0, 0]
         ])
     z = [email protected]
     return z

计算观测矩阵，具体原理见上文

输入值：

1. x ：GPS观测数据

输出值：

1. z ：观测矩阵

---

def jacobF(x, u):
 """
 Jacobian of Motion Model
 motion model
 x_{t+1} = x_t+v*dt*cos(yaw)
 y_{t+1} = y_t+v*dt*sin(yaw)
 yaw_{t+1} = yaw_t+omega*dt
 v_{t+1} = v{t}
 so
 dx/dyaw = -v*dt*sin(yaw)
 dx/dv = dt*cos(yaw)
 dy/dyaw = v*dt*cos(yaw)
 dy/dv = dt*sin(yaw)
 """
     yaw = x[2, 0]
     v = u[0, 0]
     jF = np.array([
         [1.0, 0.0, -DT * v * math.sin(yaw), DT * math.cos(yaw)],
         [0.0, 1.0, DT * v * math.cos(yaw), DT * math.sin(yaw)],
         [0.0, 0.0, 1.0, 0.0],
         [0.0, 0.0, 0.0, 1.0]])
     return jF

计算运动模型的雅各比矩阵

---

def jacobH(x):
 # Jacobian of Observation Model
     jH = np.array([
         [1, 0, 0, 0],
         [0, 1, 0, 0]
         ])
     return jH

计算观测模型的雅各比矩阵

---

def ekf_estimation(xEst, PEst, z, u):
 # Predict
     xPred = motion_model(xEst, u)
     jF = jacobF(xPred, u)
     PPred = [email protected]@jF.T + Q
 # Update
     jH = jacobH(xPred)
     zPred = observation_model(xPred)
     y = z - zPred
     S = [email protected]@jH.T + R
     K = [email protected]@np.linalg.inv(S)
     xEst = xPred + [email protected]
     PEst = (np.eye(len(xEst)) - [email protected])@PPred
     return xEst, PEst

EKF运算过程，具体流程见上文

---

def plot_covariance_ellipse(xEst, PEst):
     Pxy = PEst[0:2, 0:2]
     eigval, eigvec = np.linalg.eig(Pxy)

     if eigval[0] >= eigval[1]:
         bigind = 0
         smallind = 1
     else:
         bigind = 1
         smallind = 0

     t = np.arange(0, 2 * math.pi + 0.1, 0.1)
     a = math.sqrt(eigval[bigind])
     b = math.sqrt(eigval[smallind])
     x = [a * math.cos(it) for it in t]
     y = [b * math.sin(it) for it in t]
     angle = math.atan2(eigvec[bigind, 1], eigvec[bigind, 0])
     R = np.array([[math.cos(angle), math.sin(angle)],
         [-math.sin(angle), math.cos(angle)]])
     fx = [email protected](np.array([x, y]))
     px = np.array(fx[0, :] + xEst[0, 0]).flatten()
     py = np.array(fx[1, :] + xEst[1, 0]).flatten()
     plt.plot(px, py, "--r")

绘图函数，绘制方差概率区域

---

def main():
     print(__file__ + " start!!")

     time = 0.0

 # State Vector [x y yaw v]'
     xEst = np.zeros((4, 1))
     xTrue = np.zeros((4, 1))
     PEst = np.eye(4)

     xDR = np.zeros((4, 1)) # Dead reckoning

 # history
     hxEst = xEst
     hxTrue = xTrue
     hxDR = xTrue
     hz = np.zeros((2, 1))

     while SIM_TIME >= time:
         time += DT
         u = calc_input()

         xTrue, z, xDR, ud = observation(xTrue, xDR, u)

         xEst, PEst = ekf_estimation(xEst, PEst, z, ud)

 # store data history
         hxEst = np.hstack((hxEst, xEst))
         hxDR = np.hstack((hxDR, xDR))
         hxTrue = np.hstack((hxTrue, xTrue))
         hz = np.hstack((hz, z))

        if show_animation:
             plt.cla()
             plt.plot(hz[:, 0], hz[:, 1], ".g")
             plt.plot(hxTrue[0, :].flatten(),
                 hxTrue[1, :].flatten(), "-b")
             plt.plot(hxDR[0, :].flatten(),
                 hxDR[1, :].flatten(), "-k")
             plt.plot(hxEst[0, :].flatten(),
                 hxEst[1, :].flatten(), "-r")
             plot_covariance_ellipse(xEst, PEst)
             plt.axis("equal")
             plt.grid(True)
             plt.pause(0.001)

主运行函数
第7-11行：初始化矩阵

• xEst ：机器人状态矩阵

• xTrue ：机器人Groud Truth矩阵

• PEst ：后验误差协方差

• xDR ：航迹推测矩阵

第14-17行：历史数据存储 第19行：进入处理循环 第21行：输入控制数据 第23行：计算观测模型参数 第25行：运行EKF 第28-31行：储存历史数据 第33-45行：绘图