源码阅读-PythonRobotics-Localization-EKF
本篇笔记是对PythonRobotics中Localization部分的EKF源码进行翻译和分析,以作学习之用,如有翻译理解不对的地方,烦请大家留言指出,谢谢!
Author: Sunshengjin
Mail: [email protected]
这是此滤波器仿真图像
- 蓝色线为机器人的真实轨迹
- 黑色线为航迹推演的轨迹
- 绿色点表示GPS数据
- 红色线为EKF的推测轨迹
- 红色椭圆为EKF的估计协方差区域
过程分析
滤波器设计
在这个仿真中,机器人的状态由四个向量组成:
- 是2D的x-y位置,$ \phi v$是速度
- 在代码中,“xEST”代表的是机器人状态矩阵
- 是机器人状态协方差矩阵
- 是运行噪声协方差矩阵
- 是在时刻的观测噪声协方差矩阵
机器人安装了速度传感器和角度传感器,所以,机器人状态在时刻的输入矩阵*(input vector)* 可以写成:
机器人也安装了GNSS传感器,所以机器人可以观测到自己时刻的x-y坐标
输入矩阵与观测矩阵都包含了传感器噪声,在代码中,“观测”部分的函数生成了带有噪声的输入和观测矩阵:
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
运动模型
机器人模型为:
运动模型为:
其中:
-
是时间间隔
这一部分的代码为:
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
雅克比矩阵为:
观测模型
机器人可以从GPS传感器获取自身的x-y位置数据,GPS的观测矩阵为:
其中:
雅各比矩阵为:
扩展卡尔曼滤波部分
定位方法采用扩展卡尔曼滤波的方法,整个定位流程如下:
预测过程
更新过程
源码解析
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: 控制矩阵
返回值:
- xTrue : 机器人的Groud Truth位置
- z :向Groud Truth中叠加噪声,模拟GPS数据
- xd :机器人在有噪声的控制后的观测数据
- 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
机器人运动模型函数(传递函数),计算输入位置矩阵在输入控制矩阵作用后的机器人的位置信息,具体原理见上文
- @ : 矩阵相乘
输入值:
- x :机器人位置
- u :控制信息
输出值:
- x :在给定控制输入后,机器人的位置信息
def observation_model(x):
# Observation Model
H = np.array([
[1, 0, 0, 0],
[0, 1, 0, 0]
])
z = [email protected]
return z
计算观测矩阵,具体原理见上文
输入值:
- x :GPS观测数据
输出值:
- 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行:绘图