定位（一）：扩展卡尔曼滤波

定位是解决自主移动机器人导航中“我在哪儿？”的问题。移动机器人的定位源很多，如：GPS、航迹推算以及SLAM等等。那么当移动机器人同时具备多个定位源时，我们需要思考如何融合所有信息以得到更加准确的定位结果。今天我们要介绍一种经典的融合定位方法：扩展卡尔曼滤波。

一、卡尔曼滤波（Kalman filter, KF）

现实环境中的移动机器人存在很大不确定性（由环境、传感器、执行器、模型以及计算等的不确性造成）的事实，从贝叶斯的角度介绍并推导了KF算法。本节基于《概率机器人》¹中内容对KF进行简单介绍。

1.1 状态转移函数

状态转移函数代表系统的模型，该模型可以用来预测系统在给定控制量的未来状态轨迹。模型的准确性直接影响预测的未来状态与实际系统的状态的误差。控制领域有一句话，模型都是错的，但是我们能够得到一个接近真实系统的模型。移动机器人也存在很多被构建的模型（状态转移函数），可以被用来估计当前移动机器人的位姿或速度等状态。机器人的初始状态已知，每个控制周期的控制量（例如，加速度与方向盘转角）可以由测量单元测量得到，每个控制周期的时长同样可以由系统定时器得到，那么机器人可以通过状态转移函数估计它当前的状态，估算的状态中就包含空间位置信息。上述方法又被称为航迹推算。根据物理学中经典的“测不准”原理，多处测量的不确定性性加大了状态估计结果的随机性；并且航迹推算是以上一时刻的状态作为输入，状态的误差会随着时间不断累积。最后的最后，假定测得准、模型无误差，那么现实的计算系统是离散的；然而，现实世界的系统是连续的（模型离散计算引起的状态估计不准性详见无人车系统（一）：运动学模型及其线性化）。

假定一个系统（此处以移动机器人为例）的状态转移函数（或可线性化）为如下线性形式：

xt=Atxt−1+Btu+ϵtx_t=A_t x_{t-1}+B_t u+\epsilon_txt=Atxt−1+Btu+ϵt

其中，

xt∈Rn×1x_t\in \mathcal{R}^{n\times 1}xt∈Rn×1与xt−1∈Rn×1x_{t-1}\in \mathcal{R}^{n\times 1}xt−1∈Rn×1为状态向量；
ut∈Rm×1u_t\in \mathcal{R}^{m\times 1}ut∈Rm×1为系统在时间ttt时刻的输入控制向量；
At∈Rn×nA_t \in \mathcal{R}^{n\times n}At∈Rn×n与Bt∈Rn×mB_t \in \mathcal{R}^{n \times m}Bt∈Rn×m为参数矩阵；
ϵt∈Rn×1\epsilon_t\in \mathcal{R}^{n \times 1}ϵt∈Rn×1表示状态转移的噪声，一般假设为均值为零，方差为RtR_tRt的高斯分布；
nnn为状态量的个数，mmm为输入控制量的个数。

则可得下一状态的分布函数如下：
p(xt∣ut,xt−1)=det(2πRt)−12exp{−12(xt−Atxt−1−Btut)TRt−1(xt−Atxt−1−Btut)}p(x_t|u_t, x_{t-1})=det(2\pi R_t)^{-\frac{1}{2}}exp{\{-\frac{1}{2}(x_t-A_t x_{t-1}-B_t u_t)^T R_t^{-1}(x_t-A_t x_{t-1}-B_t u_t)\}}p(xt∣ut,xt−1)=det(2πRt)−21exp{−21(xt−Atxt−1−Btut)TRt−1(xt−Atxt−1−Btut)}

1.2 （部分可）观测函数

假定系统的状态（部分）可被观测/测量，并且测量结果ztz_tzt与状态xtx_txt的关系（或可线性化）为如下线性形式：

zt=Ctxt+δtz_t=C_t x_t+\delta_tzt=Ctxt+δt

其中，

zt∈Rk×1z_t\in \mathcal{R}^{k\times 1}zt∈Rk×1为观测向量；
Ct∈Rk×nC_t\in \mathcal{R}^{k\times n}Ct∈Rk×n为系数矩阵；
δt∈Rk×1\delta_t \in \mathcal{R}^{k\times 1}δt∈Rk×1为观测噪声，一般假设为均值为零，方差为QtQ_tQt的高斯分布；
kkk为观测量个数，当k<nk<nk<n时，表示系统是部分可观测的。

则可得下一观测的分布函数如下：
p(zt∣xt)=det(2πQt)−12exp{−12(zt−Ctxt)TQt−1(zt−Ctxt)}p(z_t|x_t)=det(2\pi Q_t)^{-\frac{1}{2}}exp{\{-\frac{1}{2}(z_t-C_t x_t)^T Q_t^{-1}(z_t-C_t x_t)\}}p(zt∣xt)=det(2πQt)−21exp{−21(zt−Ctxt)TQt−1(zt−Ctxt)}

1.3 KF算法

定义：信念(belief)反映了机器人关于环境状态的内部知识。例如，机器人状态的belief为bel(xt)=p(xt∣z1:t,u1:t)bel(x_t)=p(x_t|z_{1:t},u_{1:t})bel(xt)=p(xt∣z1:t,u1:t)，此处数学意义同后验概率分布。由bel‾=p(xt∣z1:t−1,u1:t)\overline{bel}=p(x_t|z_{1:t-1},u_{1:t})bel=p(xt∣z1:t−1,u1:t)表示考虑状态转移，未加入当前观测信息的状态后验概率。

KF算法根据初始状态x0x_0x0、每个时刻的控制量utu_tut以及观测值ztz_tzt迭代的估计状态(由均值μt\mu_tμt表示)及其不确定性（由方差Σt\Sigma_tΣt表示）。假定初始状态x0x_0x0可以由一个均值为μ0\mu_0μ0与方差为Σ0\Sigma_0Σ0，初始状态的:

bel(x0)=p(x0)=det(2πΣ0)−12exp{−12(x0−μ0)TΣ0−1(x0−μ0)}bel(x_0)=p(x_0)=det(2\pi \Sigma_0)^{-\frac{1}{2}}exp{\{-\frac{1}{2}(x_0-\mu_0)^T\Sigma_0^{-1}(x_0-\mu_0)\}}bel(x0)=p(x0)=det(2πΣ0)−21exp{−21(x0−μ0)TΣ0−1(x0−μ0)}

由如下贝叶斯滤波算法（《概率机器人P24》）：

可推导得到KF算法如下（《概率机器人》P36）：

1.4 扩展卡尔曼滤波

当状态转移函数与观测函数为如下非线性函数时：

xt=g(ut,xt−1)+ϵtx_t=g(u_t, x_{t-1})+\epsilon_t xt=g(ut,xt−1)+ϵt
zt=h(xt)+σtz_t=h(x_t)+\sigma_t zt=h(xt)+σt

对以上非线性函数进行泰勒展开，可化为线性形式。所以，扩展卡尔曼滤波算法只比原始卡尔曼滤波算法多了一步线性化的工作。算法形式一模一样。具体EKF算法如下：

二、移动机器人定位

2.1 滤波器设计

本篇本节以非完整约束的单车模型为例，来设计KF。此处，移动机器人在时间ttt时刻的状态为四维向量xt\mathbf{x}_txt：
xt=[xt,yt,ϕt,vt]\mathbf{x}_t=[x_t, y_t, \phi_t,v_t]xt=[xt,yt,ϕt,vt]

xtx_txt为二维笛卡尔坐标系的X轴坐标值；
yty_tyt为二维笛卡尔坐标系的Y轴坐标值；
ϕt\phi_tϕt为移动机器的航向角；
vtv_tvt为移动机器人的行驶线速度。

机器人的状态方差为Σt\Sigma_tΣt，状态转移噪声方差为RRR，观测噪声方差为QQQ。

移动机器人装载有速度传感器与陀螺仪，可分别测量时刻ttt的速度与角速度为:
ut=[vt,ωt]\mathbf{u}_t=[v_t, \omega_t]ut=[vt,ωt]

同样，假定移动机器人装载有GNSS（卫星导航定位）传感器，可以实时获得自身的位置作为自身的观测向量zt\mathbf{z}_tzt

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

2.2 移动机器人运动模型

连续运动学模型：

{x˙=vcos(ϕ)y˙=vsin(ϕ)ϕ˙=ω}(1)\left\{\begin{matrix} \dot{x}=vcos(\phi) \\ \dot{y}=vsin(\phi) \\ \dot{\phi}=\omega \end{matrix}\right\}\tag{1} ⎩⎨⎧x˙=vcos(ϕ)y˙=vsin(ϕ)ϕ˙=ω⎭⎬⎫(1)

经过离散化后得如下形式：
{xt=xt−1+dtvcos(ϕ)yt=yt−1+dtvsin(ϕ)ϕt=ϕt−1+dtωt}(2)\left\{\begin{matrix} x_t=x_{t-1}+d_t vcos(\phi) \\ y_t=y_{t-1}+d_t vsin(\phi) \\ \phi_t=\phi_{t-1}+d_t \omega_t \end{matrix}\right\}\tag{2} ⎩⎨⎧xt=xt−1+dtvcos(ϕ)yt=yt−1+dtvsin(ϕ)ϕt=ϕt−1+dtωt⎭⎬⎫(2)
状态向量为xt=[xt,yt,ϕt,vt]T\mathbf{x}_t=[x_t, y_t, \phi_t, v_t]^Txt=[xt,yt,ϕt,vt]T，输入控制量为ut=[vt,ωt]\mathbf{u}_t=[v_t, \omega_t]ut=[vt,ωt]，可得状态转移函数为：

xt=Axt−1+But\mathbf{x}_t=A \mathbf{x}_{t-1}+B\mathbf{u}_t xt=Axt−1+But

其中，
A=[1.000001.000001.000000]A=\left[\begin{matrix} 1.0 & 0 & 0 & 0 \\ 0 & 1.0 & 0 & 0 \\ 0 & 0 & 1.0 & 0 \\ 0 & 0 & 0 & 0 \end{matrix}\right]A=⎣⎢⎢⎡1.000001.000001.000000⎦⎥⎥⎤

B=[dtcos⁡(ϕt)0dtsin⁡(ϕt)00dt1.00]B=\left[\begin{matrix} d_t \cos(\phi_t) & 0 \\ d_t \sin(\phi_t) & 0 \\ 0 & d_t \\ 1.0 & 0 \end{matrix}\right]B=⎣⎢⎢⎡dtcos(ϕt)dtsin(ϕt)01.000dt0⎦⎥⎥⎤

Jacobian矩阵
由于控制向量的系数矩阵BBB中包含状态量ϕt\phi_tϕt，所以上面的状态转移函数为非线性。此处需要利用式（2）求得状态函数的Jacobian矩阵GtG_tGt：

Gt=[dxdxdxdydxdϕdxdvdydxdydydydϕdydvdϕdxdϕdydϕdϕdϕdvdvdxdvdydvdϕdvdv]G_t= \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} Gt=⎣⎢⎢⎢⎡dxdxdxdydxdϕdxdvdydxdydydydϕdydvdϕdxdϕdydϕdϕdϕdvdvdxdvdydvdϕdvdv⎦⎥⎥⎥⎤
=[10−vsin(ϕ)dtcos(ϕ)dt01vcos(ϕ)dtsin(ϕ)dt00100001]= \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} 　=⎣⎢⎢⎡10000100−vsin(ϕ)dtvcos(ϕ)dt10cos(ϕ)dtsin(ϕ)dt01⎦⎥⎥⎤

2.3 移动机器人观测模型

机器人可以从GPS中获得x-y位置信息。可得观测模型为：

zt=Cxt\mathbf{z}_t=C\mathbf{x}_t zt=Cxt

C=[1.000001.000]C=\left[\begin{matrix} 1.0 & 0 & 0 & 0 \\ 0 & 1.0 & 0 & 0 \end{matrix}\right]C=[1.0001.00000]

三、python代码

代码参照AtsushiSakai/PythonRobotics

import math
import matplotlib
import matplotlib.pyplot as plt
%matplotlib inline# set up matplotlib
is_ipython = 'inline' in matplotlib.get_backend()
if is_ipython:from IPython import displayplt.ion()import numpy as np
from scipy.spatial.transform import Rotation as Rot# Covariance for EKF simulation
R = np.diag([0.1,  # variance of location on x-axis0.1,  # variance of location on y-axisnp.deg2rad(1.0),  # variance of yaw angle1.0  # variance of velocity
]) ** 2  # predict state covariance
Q = 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):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):A = 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 = A @ x + B @ ureturn xdef observation_model(x):C = np.array([[1, 0, 0, 0],[0, 1, 0, 0]])z = C @ xreturn zdef jacob_f(x, u):yaw = x[2, 0]v = u[0, 0]G = 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 Gdef jacob_h():# Jacobian of Observation ModelC = np.array([[1, 0, 0, 0],[0, 1, 0, 0]])return Cdef ekf_estimation(xEst, PEst, z, u):#  PredictxPred = motion_model(xEst, u)G = jacob_f(xEst, u)PPred = G @ PEst @ G.T + R#  UpdateC = jacob_h()zPred = observation_model(xPred)S = C @ PPred @ C.T + QK = PPred @ C.T @ np.linalg.inv(S)xEst = xPred + K @ (z - zPred)PEst = (np.eye(len(xEst)) - K @ C) @ 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[1, bigind], eigvec[0, bigind])rot = Rot.from_euler('z', angle).as_matrix()[0:2, 0:2]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():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# 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 is_ipython:display.clear_output(wait=True)display.display(plt.gcf())            if __name__ == '__main__':main()

运行结果如下图所示。

图中，黑色的轨迹表示由移动机器人航迹推算出的位姿结果，绿点表示由GPS得到的有噪声的定位信息，红色轨迹表示由EKF估计的定位结果。蓝色曲线表示定位真值，基本与红色轨迹重合。很明显，在整个过程中，作为观测值的GPS定位信息是作为基准信息，状态转移函数的作用是降低了GPS定位信息中的噪声。

四、结语

重点：滤波是滤谁的波（航迹推算滤观测的噪声），观测与部分可观测的定义（当观测量的个数比状态量的个数要少时），这些都是最近恍然感悟到的。

Ref:

PROBABILISTIC-ROBOTICS.ORG ↩︎