EKF SLAM学习笔记02

2 扩展卡尔曼滤波EKF

在上一节中，我们了解到了卡尔曼滤波的计算公式。卡尔曼滤波基于线性系统的假设，如果运动模型或者观测模型不能用线性系统来表示（大部分现实问题都无法遵从线性系统的假设），那么我们仍然可以使用卡尔曼滤波的思想，只不过我们使用一阶雅克比矩阵来代替状态转移矩阵来进行计算（证明略），这就是扩展卡尔曼滤波EKF：

Localization process using Extendted Kalman Filter:EKF is

=== Predict ===

xPred=Fxt+Butx_{Pred} = Fx_t+Bu_txPred=Fxt+But

PPred=JFPtJFT+QP_{Pred} = J_FP_t J_F^T + QPPred=JFPtJFT+Q

=== Update ===

zPred=HxPredz_{Pred} = Hx_{Pred}zPred=HxPred

y=z−zPredy = z - z_{Pred}y=z−zPred

S=JHPPred.JHT+RS = J_H P_{Pred}.J_H^T + RS=JHPPred.JHT+R

K=PPred.JHTS−1K = P_{Pred}.J_H^T S^{-1}K=PPred.JHTS−1

xt+1=xPred+Kyx_{t+1} = x_{Pred} + Kyxt+1=xPred+Ky

Pt+1=(I−KJH)PPredP_{t+1} = ( I - K J_H) P_{Pred}Pt+1=(I−KJH)PPred

注：首先，在以上公式中，xPred=Fxt+Butx_{Pred} = Fx_t+Bu_txPred=Fxt+But的一种更普遍的表达方式是：xPred=g(xt,ut)x_{Pred} = g(x_t,u_t)xPred=g(xt,ut)，以表现运动模型的非线性。在此继续采用Fxt+ButFx_t+Bu_tFxt+But矩阵相乘的形式是为了对应下面的python代码，实际上我们可以看到在代码中运动模型还是非线性的（原因是BBB矩阵包含了状态量）。同样地，zPred=HxPredz_{Pred} = Hx_{Pred}zPred=HxPred 的一种更普遍的表达方式是 zPred=h(xPred)z_{Pred} = h(x_{Pred})zPred=h(xPred)，在此采用矩阵相乘形式是为了对应下面的python代码。

上面公式中，JFJ_FJF代表了运动模型的雅克比矩阵，JHJ_HJH代表了观测模型的雅克比矩阵。扩展卡尔曼滤波

下面通过一个python实例来展示EKF的核心概念。（完整代码见原链接，有中文注释的附在了最后）

图中是一个应用Extended Kalman Filter(EKF)做传感器融合定位的实例。

蓝色线是轨迹真值，黑色线是“航迹推测”得出的轨迹（航迹推测指的是单纯凭借速度信息推测位置的方法，会将速度信息的误差包含在位置中）

绿色点是观测值(ex. GPS), 红色线是经过EKF滤波后的轨迹。

红色椭圆代表EKF输出的机器人状态的实时协方差。

源代码链接: PythonRobotics/extended_kalman_filter.py at master · AtsushiSakai/PythonRobotics

滤波器设计

在这个实例中，二维机器人具有一个四个元素的状态向量：

xt=[xt,yt,ϕt,vt]\textbf{x}_t=[x_t, y_t, \phi_t, v_t]xt=[xt,yt,ϕt,vt]

x, y 是 2D x-y 位置, ϕ\phiϕ 是角度, v 是线速度.

在代码中, 用"xEst" 代表状态向量. code

PtP_tPt 代表状态的协方差矩阵，

QQQ 运动模型噪声的协方差矩阵，

RRR 观测模型噪声的协方差矩阵，具体见代码注释（注意，在这个实例中，QQQ和RRR都是固定的）

机器人装备有线速度传感器和角速度传感器，因此运动模型的控制输入向量可以用线速度和角速度表示：

ut=[vt,ωt]\textbf{u}_t=[v_t, \omega_t]ut=[vt,ωt]

机器人还有 GNSS 传感器（相当于GPS），意味着机器人可以获得x-y位置定位的观测值：

zt=[xt,yt]\textbf{z}_t=[x_t,y_t]zt=[xt,yt]

别忘了，为了模拟实际情况，控制输入向量和观测值向量都应该带有噪声。

在源代码中, “observation” 函数通过在理论值上人为加随机噪声来模拟实际信号code

def observation(xTrue, xd, u):"""执行仿真过程，不是EKF的一部分"""# 轨迹真值xTrue = motion_model(xTrue, u)# add noise to gps x-yz = observation_model(xTrue) + GPS_NOISE @ np.random.randn(2, 1)# add noise to inputud = u + INPUT_NOISE @ np.random.randn(2, 1)# 航迹推测得出的轨迹：xd = motion_model(xd, ud)return xTrue, z, xd, ud

运动模型

机器人的运动模型为：

x˙=vcos(ϕ)\dot{x} = vcos(\phi)x˙=vcos(ϕ)

y˙=vsin((ϕ)\dot{y} = vsin((\phi)y˙=vsin((ϕ)

ϕ˙=ω\dot{\phi} = \omegaϕ˙=ω

运动模型方程：

xt+1=Fxt+But\textbf{x}_{t+1} = F\textbf{x}_t+B\textbf{u}_txt+1=Fxt+But

其中，

F=[1000010000100000]\begin{aligned} F= \begin{bmatrix} 1 & 0 & 0 & 0\\ 0 & 1 & 0 & 0\\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 0 \\ \end{bmatrix} \end{aligned}F=⎣⎢⎢⎡1000010000100000⎦⎥⎥⎤

B=[cos(ϕ)dt0sin(ϕ)dt00dt10]\begin{aligned} B= \begin{bmatrix} cos(\phi)dt & 0\\ sin(\phi)dt & 0\\ 0 & dt\\ 1 & 0\\ \end{bmatrix} \end{aligned}B=⎣⎢⎢⎡cos(ϕ)dtsin(ϕ)dt0100dt0⎦⎥⎥⎤

dtdtdt 是时间步长。

注意实际上矩阵BBB包含了状态向量（机器人角度），这不再是一个单纯的线性模型。

这部分的源代码： code

运动模型的雅克比矩阵（也就是按照状态向量中的各个元素依次彼此求一阶偏导）为：

JF=[dxdxdxdydxdϕdxdvdydxdydydydϕdydvdϕdxdϕdydϕdϕdϕdvdvdxdvdydvdϕdvdv]\begin{aligned} 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} \end{aligned}JF=⎣⎢⎢⎢⎡dxdxdxdydxdϕdxdvdydxdydydydϕdydvdϕdxdϕdydϕdϕdϕdvdvdxdvdydvdϕdvdv⎦⎥⎥⎥⎤

=[10−vsin(ϕ)dtcos(ϕ)dt01vcos(ϕ)dtsin(ϕ)dt00100001]\begin{aligned} 　= \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} \end{aligned}　=⎣⎢⎢⎡10000100−vsin(ϕ)dtvcos(ϕ)dt10cos(ϕ)dtsin(ϕ)dt01⎦⎥⎥⎤

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 @ ureturn xdef jacob_f(x, u):"""Jacobian of Motion Modelmotion modelx_{t+1} = x_t+v*dt*cos(yaw)y_{t+1} = y_t+v*dt*sin(yaw)yaw_{t+1} = yaw_t+omega*dtv_{t+1} = v{t}sodx/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的观测模型是：

zt=Hxt\textbf{z}_{t} = H\textbf{x}_tzt=Hxt

其中

H=[10000100]\begin{aligned} H= \begin{bmatrix} 1 & 0 & 0& 0\\ 0 & 1 & 0& 0\\ \end{bmatrix} \end{aligned}H=[10010000]

（即直接获取某时刻x、y的值，与角度速度无关）

对应的雅克比矩阵：

JH=[dxdxdxdydxdϕdxdvdydxdydydydϕdydv]\begin{aligned} 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} \end{aligned}JH=[dxdxdxdydydxdydydϕdxdϕdydvdxdvdy]

=[10000100]\begin{aligned} 　= \begin{bmatrix} 1& 0 & 0 & 0\\ 0 & 1 & 0 & 0\\ \end{bmatrix} \end{aligned}　=[10010000]

def observation_model(x):H = np.array([[1, 0, 0, 0],[0, 1, 0, 0]])z = H @ xreturn zdef jacob_h():# Jacobian of Observation ModeljH = np.array([[1, 0, 0, 0],[0, 1, 0, 0]])return jH

以下是完整程序中文注释版：

"""Extended kalman filter (EKF) localization sampleauthor: Atsushi Sakai (@Atsushi_twi)https://github.com/AtsushiSakai/PythonRobotics/blob/master/Localization/extended_kalman_filter/
"""import mathimport matplotlib.pyplot as plt
import numpy as np# Covariance for EKF:
# 运动模型协方差：
Q = np.diag([0.1,  # variance of location on x-axis0.1,  # variance of location on y-axisnp.deg2rad(10),  # variance of yaw angle10.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]) ** 2DT = 0.1  # time tick [s]
SIM_TIME = 50.0  # simulation time [s]show_animation = Truedef calc_input():v = 1.0  # [m/s]yawrate = 0.1  # [rad/s]u = np.array([[v], [yawrate]])return udef observation(xTrue, xd, u):"""执行仿真过程，不是EKF的一部分"""# 轨迹真值xTrue = motion_model(xTrue, u)# add noise to gps x-yz = observation_model(xTrue) + GPS_NOISE @ np.random.randn(2, 1)# add noise to inputud = u + INPUT_NOISE @ np.random.randn(2, 1)# 航迹推测得出的轨迹：xd = motion_model(xd, ud)return xTrue, z, xd, uddef 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 @ ureturn xdef observation_model(x):H = np.array([[1, 0, 0, 0],[0, 1, 0, 0]])z = H @ xreturn zdef jacob_f(x, u):"""Jacobian of Motion Modelmotion modelx_{t+1} = x_t+v*dt*cos(yaw)y_{t+1} = y_t+v*dt*sin(yaw)yaw_{t+1} = yaw_t+omega*dtv_{t+1} = v{t}sodx/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 jFdef jacob_h():# Jacobian of Observation ModeljH = np.array([[1, 0, 0, 0],[0, 1, 0, 0]])return jHdef ekf_estimation(xEst, PEst, z, u):#  PredictxPred = motion_model(xEst, u)jF = jacob_f(xEst, u)PPred = jF @ PEst @ jF.T + Q#  UpdatejH = jacob_h()zPred = observation_model(xPred)y = z - zPredS = jH @ PPred @ jH.T + RK = PPred @ jH.T @ np.linalg.inv(S)xEst = xPred + K @ yPEst = (np.eye(len(xEst)) - K @ jH) @ PPredreturn xEst, PEstdef plot_covariance_ellipse(xEst, PEst):  # pragma: no coverPxy = PEst[0:2, 0:2]eigval, eigvec = np.linalg.eig(Pxy)if eigval[0] >= eigval[1]:bigind = 0smallind = 1else:bigind = 1smallind = 0t = 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)) # 初始值全部为0xTrue = np.zeros((4, 1))PEst = np.eye(4) #用一个对角都是1的矩阵表示状态协方差矩阵初始值xDR = np.zeros((4, 1))  # Dead reckoning# historyhxEst = xEsthxTrue = xTruehxDR = xTruehz = np.zeros((2, 1))while SIM_TIME >= time:time += DTu = calc_input()xTrue, z, xDR, ud = observation(xTrue, xDR, u)xEst, PEst = ekf_estimation(xEst, PEst, z, ud)# store data historyhxEst = 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()