2 扩展卡尔曼滤波EKF
在上一节中,我们了解到了卡尔曼滤波的计算公式。卡尔曼滤波基于线性系统的假设,如果运动模型或者观测模型不能用线性系统来表示(大部分现实问题都无法遵从线性系统的假设),那么我们仍然可以使用卡尔曼滤波的思想,只不过我们使用一阶雅克比矩阵来代替状态转移矩阵来进行计算(证明略),这就是扩展卡尔曼滤波EKF:
Localization process using Extendted Kalman Filter:EKF is
=== Predict ==
下面通过一个python实例来展示EKF的核心概念。(完整代码见原链接,有中文注释的附在了最后)
图中是一个应用Extended Kalman Filter(EKF)做传感器融合定位的实例。
蓝色线是轨迹真值,黑色线是“航迹推测”得出的轨迹(航迹推测指的是单纯凭借速度信息推测位置的方法,会将速度信息的误差包含在位置中)
绿色点是观测值(ex. GPS), 红色线是经过EKF滤波后的轨迹。
红色椭圆代表EKF输出的机器人状态的实时协方差。
源代码链接: PythonRobotics/extended_kalman_filter.py at master · AtsushiSakai/PythonRobotics
滤波器设计
在这个实例中,二维机器人具有一个四个元素的状态向量:
别忘了,为了模拟实际情况,控制输入向量和观测值向量都应该带有噪声。
在源代码中, “observation” 函数通过在理论值上人为加随机噪声来模拟实际信号code
def observation(xTrue, xd, u):
"""
执行仿真过程,不是EKF的一部分
"""
# 轨迹真值
xTrue = motion_model(xTrue, u)
# add noise to gps x-y
z = observation_model(xTrue) + GPS_NOISE @ np.random.randn(2, 1)
# add noise to input
ud = u + INPUT_NOISE @ np.random.randn(2, 1)
# 航迹推测得出的轨迹:
xd = motion_model(xd, ud)
return xTrue, z, xd, ud
运动模型
机器人的运动模型为:
dt 是时间步长。
注意实际上矩阵B 包含了状态向量(机器人角度),这不再是一个单纯的线性模型。
这部分的源代码: code
运动模型的雅克比矩阵(也就是按照状态向量中的各个元素依次彼此求一阶偏导)为:
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矩阵中耦合了状态向量x,因此并不是简单的线性模型:
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 @ x + B @ u
return x
def jacob_f(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
观测模型
在这个实例中,机器人可以通过GPS直接获取二维 x-y 坐标。
所以GPS的观测模型是:
def observation_model(x):
H = np.array([
[1, 0, 0, 0],
[0, 1, 0, 0]
])
z = H @ x
return z
def jacob_h():
# Jacobian of Observation Model
jH = np.array([
[1, 0, 0, 0],
[0, 1, 0, 0]
])
return jH
以下是完整程序中文注释版:
"""
Extended kalman filter (EKF) localization sample
author: Atsushi Sakai (@Atsushi_twi)
https://github.com/AtsushiSakai/PythonRobotics/blob/master/Localization/extended_kalman_filter/
"""
import math
import matplotlib.pyplot as plt
import numpy as np
# Covariance for EKF:
# 运动模型协方差:
Q = np.diag([
0.1, # variance of location on x-axis
0.1, # variance of location on y-axis
np.deg2rad(10), # variance of yaw angle
10.0 # variance of velocity
]) ** 2 # predict state covariance
# 观测模型协方差:
R = np.diag([1.0, 1.0]) ** 2 # Observation x,y position covariance
# Simulation parameter
INPUT_NOISE = np.diag([1.0, np.deg2rad(30.0)]) ** 2
GPS_NOISE = np.diag([0.5, 0.5]) ** 2
DT = 0.1 # time tick [s]
SIM_TIME = 50.0 # simulation time [s]
show_animation = True
def calc_input():
v = 1.0 # [m/s]
yawrate = 0.1 # [rad/s]
u = np.array([[v], [yawrate]])
return u
def observation(xTrue, xd, u):
"""
执行仿真过程,不是EKF的一部分
"""
# 轨迹真值
xTrue = motion_model(xTrue, u)
# add noise to gps x-y
z = observation_model(xTrue) + GPS_NOISE @ np.random.randn(2, 1)
# add noise to input
ud = u + INPUT_NOISE @ np.random.randn(2, 1)
# 航迹推测得出的轨迹:
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矩阵中耦合了状态向量x,因此并不是简单的线性模型:
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 @ x + B @ u
return x
def observation_model(x):
H = np.array([
[1, 0, 0, 0],
[0, 1, 0, 0]
])
z = H @ x
return z
def jacob_f(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 jacob_h():
# 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 = jacob_f(xEst, u)
PPred = jF @ PEst @ jF.T + Q
# Update
jH = jacob_h()
zPred = observation_model(xPred)
y = z - zPred
S = jH @ PPred @ jH.T + R
K = PPred @ jH.T @ np.linalg.inv(S)
xEst = xPred + K @ y
PEst = (np.eye(len(xEst)) - K @ jH) @ PPred
return xEst, PEst
def plot_covariance_ellipse(xEst, PEst): # pragma: no cover
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])
rot = np.array([[math.cos(angle), math.sin(angle)],
[-math.sin(angle), math.cos(angle)]])
fx = rot @ (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)) # 初始值全部为0
xTrue = np.zeros((4, 1))
PEst = np.eye(4) #用一个对角都是1的矩阵表示状态协方差矩阵初始值
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()
# for stopping simulation with the esc key.
plt.gcf().canvas.mpl_connect('key_release_event',
lambda event: [exit(0) if event.key == 'escape' else None])
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)
if __name__ == '__main__':
main()
评论(0)
您还未登录,请登录后发表或查看评论