PythonRobotics机器人算法-定位算法之扩展/无损卡尔曼滤波定位(Extended/Unscented Kalman Filter localization)


1.概述

卡尔曼滤波(Kalman filter)是一种高效率的递归滤波器(自回归滤波器),它能够从一系列的不完全及包含噪声的测量中,估计动态系统的状态。卡尔曼滤波会根据各测量量在不同时间下的值,考虑各时间下的联合分布,再产生对未知变数的估计,因此会以只以单一测量量为基础的估计方式要准。卡尔曼滤波得名自主要贡献者之一的鲁道夫·卡尔曼。

卡尔曼滤波在技术领域有许多的应用。常见的有飞机及太空船的导引、导航及控制[1]。卡尔曼滤波也广为使用在时间序列的分析中,例如信号处理及计量经济学中。卡尔曼滤波也是机器人运动规划及控制的重要主题之一,有时也包括在轨迹最佳化。卡尔曼滤波也用在中轴神经系统运动控制的建模中。因为从给与运动命令到收到感觉神经的回授之间有时间差,使用卡尔曼滤波有助于建立符合实际的系统,估计运动系统的目前状态,并且更新命令[2]

卡尔曼滤波的算法是二步骤的程序。在估计步骤中,卡尔曼滤波会产生有关目前状态的估计,其中也包括不确定性。只要观察到下一个量测(其中一定含有某种程度的误差,包括随机噪声)。会透过加权平均来更新估计值,而确定性越高的量测加权比重也越高。算法是迭代的,可以在实时控制系统中执行,只需要目前的输入量测、以往的计算值以及其不确定性矩阵,不需要其他以往的信息。

使用卡尔曼滤波不用假设误差是正态分布[3],不过若所有的误差都是正态分布,卡尔曼滤波可以得到正确的条件机率估计。

也发展了一些扩展或是广义的卡尔曼滤波,例如运作在非线性糸统的扩展卡尔曼滤波及无损卡尔曼滤波(unscented Kalman filter)。底层的模型类似隐马尔可夫模型,不过潜在变量的状态空间是连续的,而且所有潜在变量及可观测变数都是正态分布。

卡尔曼滤波的一个典型实例是从一组有限的,包含噪声的,通过对物体位置的观察序列(可能有偏差)预测出物体的位置的坐标及速度。在很多工程应用(如雷达、电脑视觉)中都可以找到它的身影。同时,卡尔曼滤波也是控制理论以及控制系统工程中的一个重要课题。例如,对于雷达来说,人们感兴趣的是其能够跟踪目标。但目标的位置、速度、加速度的测量值往往在任何时候都有噪声。卡尔曼滤波利用目标的动态信息,设法去掉噪声的影响,得到一个关于目标位置的好的估计。这个估计可以是对当前目标位置的估计(滤波),也可以是对于将来位置的估计(预测),也可以是对过去位置的估计(插值或平滑)。

2.实例

2.1扩展卡尔曼滤波定位(Extended Kalman Filter localization)

基本卡尔曼滤波器(Kalman Filter,KF)是限制在线性的假设之下,但大量系统都是非线性系统。而扩展卡尔曼滤波(Extended Kalman Filter,EKF)的中心思想就是将非线性系统线形化后再做KF处理。

(1)定义初始参数

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
import numpy as np
import matplotlib.pyplot as plt
import math

#Estimation parameter of EKF
Q = np.diag([0.1, 0.1, math.radians(1.0), 1.0])* * 2
R = np.diag([1.0, math.radians(40.0)])* * 2

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

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

show_animation = True

(2)算法过程

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
def calc_input():
    v = 1.0  # [m/s]
    yawrate = 0.1  # [rad/s]
    u = np.matrix([v, yawrate]).T
    return u

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.matrix([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.matrix([ud1, ud2]).T
    xd = motion_model(xd, ud)
    return xTrue, z, xd, ud

def motion_model(x, u):
    F = np.matrix([[1.0, 0, 0, 0],
                   [0, 1.0, 0, 0],
                   [0, 0, 1.0, 0],
                   [0, 0, 0, 0]])
    B = np.matrix([[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):
    #Observation Model
    H = np.matrix([
        [1, 0, 0, 0],
        [0, 1, 0, 0]
    ])
    z = H * x
    return z

def jacobF(x, u):
    yaw = x[2, 0]
    v = u[0, 0]
    jF = np.matrix([
        [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.matrix([
        [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 = jF * PEst * jF.T + Q
    #Update
    jH = jacobH(xPred)
    zPred = observation_model(xPred)
    y = z.T - 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

(3)结果可视化

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
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.matrix([[math.cos(angle), math.sin(angle)],
                   [-math.sin(angle), math.cos(angle)]])
    fx = R * np.matrix([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.matrix(np.zeros((4, 1)))
    xTrue = np.matrix(np.zeros((4, 1)))
    PEst = np.eye(4)
    xDR = np.matrix(np.zeros((4, 1)))  # Dead reckoning
    # history
    hxEst = xEst
    hxTrue = xTrue
    hxDR = xTrue
    hz = np.zeros((1, 2))
    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.vstack((hz, z))
        if show_animation:
            plt.cla()
            plt.plot(hz[:, 0], hz[:, 1], ".g")
            plt.plot(np.array(hxTrue[0, :]).flatten(),
                     np.array(hxTrue[1, :]).flatten(), "-b")
            plt.plot(np.array(hxDR[0, :]).flatten(),
                     np.array(hxDR[1, :]).flatten(), "-k")
            plt.plot(np.array(hxEst[0, :]).flatten(),
                     np.array(hxEst[1, :]).flatten(), "-r")
            plot_covariance_ellipse(xEst, PEst)
            plt.axis("equal")
            plt.grid(True)
            plt.pause(0.001)

if _ _ name _ _ == '_ _ main _ _':
    main()

执行结果如下。该算法利用扩展卡尔曼滤波实现传感器融合定位。蓝色曲线为实际轨迹,黑色曲线为航位推算轨迹,绿色点为定位的观察点(比如GPS),红色曲线为扩展卡尔曼滤波算法估算的轨迹。红色椭圆为基于扩展卡尔曼滤波方法估计的协方差椭圆。

2.2无损卡尔曼滤波定位(Unscented Kalman Filter localization)

EKF利用泰勒展开将非线性系统线性化。可是,EKF在强非线性系统下的误差很大。本节的无损卡尔曼滤波UKF(Unscented Kalman Filter),其计算精度相比EKF更高并省略了Jacobian矩阵的计算。

(1)定义初始参数

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
import numpy as np
import scipy.linalg
import math
import matplotlib.pyplot as plt

#Estimation parameter of UKF
Q = np.diag([0.1, 0.1, math.radians(1.0), 1.0])* * 2
R = np.diag([1.0, math.radians(40.0)])* * 2

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

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

#UKF Parameter
ALPHA = 0.001
BETA = 2
KAPPA = 0

show_animation = True

(2)算法过程

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
def calc_input():
    v = 1.0  # [m/s]
    yawrate = 0.1  # [rad/s]
    u = np.matrix([v, yawrate]).T
    return u

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.matrix([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.matrix([ud1, ud2]).T
    xd = motion_model(xd, ud)
    return xTrue, z, xd, ud

def motion_model(x, u):
    F = np.matrix([[1.0, 0, 0, 0],
                   [0, 1.0, 0, 0],
                   [0, 0, 1.0, 0],
                   [0, 0, 0, 0]])
    B = np.matrix([[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):
    #Observation Model
    H = np.matrix([
        [1, 0, 0, 0],
        [0, 1, 0, 0]
    ])
    z = H * x
    return z

def generate_sigma_points(xEst, PEst, gamma):
    sigma = xEst
    Psqrt = np.matrix(scipy.linalg.sqrtm(PEst))
    n = len(xEst[:, 0])
    #Positive direction
    for i in range(n):
        sigma = np.hstack((sigma, xEst + gamma * Psqrt[:, i]))
    #Negative direction
    for i in range(n):
        sigma = np.hstack((sigma, xEst - gamma * Psqrt[:, i]))
    return sigma

def predict_sigma_motion(sigma, u):
    #Sigma Points predition with motion model
    for i in range(sigma.shape[1]):
        sigma[:, i] = motion_model(sigma[:, i], u)
    return sigma

def predict_sigma_observation(sigma):
    # Sigma Points predition with observation model
    for i in range(sigma.shape[1]):
        sigma[0:2, i] = observation_model(sigma[:, i])
    sigma = sigma[0:2, :]
    return sigma

def calc_sigma_covariance(x, sigma, wc, Pi):
    nSigma = sigma.shape[1]
    d = sigma - x[0:sigma.shape[0], :]
    P = Pi
    for i in range(nSigma):
        P = P + wc[0, i] * d[:, i] * d[:, i].T
    return P

def calc_pxz(sigma, x, z_sigma, zb, wc):
    #function P=CalcPxz(sigma,xPred,zSigma,zb,wc)
    nSigma = sigma.shape[1]
    dx = np.matrix(sigma - x)
    dz = np.matrix(z_sigma - zb[0:2, :])
    P = np.matrix(np.zeros((dx.shape[0], dz.shape[0])))
    for i in range(nSigma):
        P = P + wc[0, i] * dx[:, i] * dz[:, i].T
    return P

def ukf_estimation(xEst, PEst, z, u, wm, wc, gamma):
    #Predict
    sigma = generate_sigma_points(xEst, PEst, gamma)
    sigma = predict_sigma_motion(sigma, u)
    xPred = (wm * sigma.T).T
    PPred = calc_sigma_covariance(xPred, sigma, wc, Q)
    #Update
    zPred = observation_model(xPred)
    y = z.T - zPred
    sigma = generate_sigma_points(xPred, PPred, gamma)
    zb = (wm * sigma.T).T
    z_sigma = predict_sigma_observation(sigma)
    st = calc_sigma_covariance(zb, z_sigma, wc, R)
    Pxz = calc_pxz(sigma, xPred, z_sigma, zb, wc)
    K = Pxz * np.linalg.inv(st)
    xEst = xPred + K * y
    PEst = PPred - K * st * K.T
    return xEst, PEst

(3)结果可视化

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
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.matrix([[math.cos(angle), math.sin(angle)],
                   [-math.sin(angle), math.cos(angle)]])
    fx = R * np.matrix([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 setup_ukf(nx):
    lamda = ALPHA ** 2 * (nx + KAPPA) - nx
    #calculate weights
    wm = [lamda / (lamda + nx)]
    wc = [(lamda / (lamda + nx)) + (1 - ALPHA ** 2 + BETA)]
    for i in range(2 * nx):
        wm.append(1.0 / (2 * (nx + lamda)))
        wc.append(1.0 / (2 * (nx + lamda)))
    gamma = math.sqrt(nx + lamda)
    wm = np.matrix(wm)
    wc = np.matrix(wc)
    return wm, wc, gamma

def main():
    print(_ _ file _ _ + " start!!")
    nx = 4  # State Vector [x y yaw v]'
    xEst = np.matrix(np.zeros((nx, 1)))
    xTrue = np.matrix(np.zeros((nx, 1)))
    PEst = np.eye(nx)
    xDR = np.matrix(np.zeros((nx, 1)))  # Dead reckoning
    wm, wc, gamma = setup_ukf(nx)
    #history
    hxEst = xEst
    hxTrue = xTrue
    hxDR = xTrue
    hz = np.zeros((1, 2))
    time = 0.0
    while SIM_TIME >= time:
        time += DT
        u = calc_input()
        xTrue, z, xDR, ud = observation(xTrue, xDR, u)
        xEst, PEst = ukf_estimation(xEst, PEst, z, ud, wm, wc, gamma)
        #store data history
        hxEst = np.hstack((hxEst, xEst))
        hxDR = np.hstack((hxDR, xDR))
        hxTrue = np.hstack((hxTrue, xTrue))
        hz = np.vstack((hz, z))
        if show_animation:
            plt.cla()
            plt.plot(hz[:, 0], hz[:, 1], ".g")
            plt.plot(np.array(hxTrue[0, :]).flatten(),
                     np.array(hxTrue[1, :]).flatten(), "-b")
            plt.plot(np.array(hxDR[0, :]).flatten(),
                     np.array(hxDR[1, :]).flatten(), "-k")
            plt.plot(np.array(hxEst[0, :]).flatten(),
                     np.array(hxEst[1, :]).flatten(), "-r")
            plot_covariance_ellipse(xEst, PEst)
            plt.axis("equal")
            plt.grid(True)
            plt.pause(0.001)

if _ _ name _ _ == '_ _ main _ _':
    main()

执行结果如下。该算法利用无损卡尔曼滤波实现传感器融合定位。图中曲线与点的意义同2.1中定义。

参考文献

  1. Musoff H, Zarchan P. Fundamentals of Kalman filtering: a practical approach[M]. American Institute of Aeronautics and Astronautics, 2009.
  2. Wolpert D M, Ghahramani Z. Computational principles of movement neuroscience[J]. Nature neuroscience, 2000, 3(11s): 1212.
  3. Kalman R E. A new approach to linear filtering and prediction problems[J]. Journal of basic Engineering, 1960, 82(1): 35-45.
  4. https://zh.wikipedia.org/wiki/卡尔曼滤波
  5. https://pythonrobotics.readthedocs.io/en/latest/
  6. https://github.com/AtsushiSakai/PythonRobotics/
  7. PythonRobotics: a Python code collection of robotics algorithms
  8. http://www.probabilistic-robotics.org/
  9. Sakai A, Kuroda Y. Discriminatively Trained Unscented Kalman Filter for Mobile Robot Localization[J]. Journal of Advanced Research in Mechanical Engineering, 2010, 1(3).

进化学习团队将会根据大家意见和建议持续修改、维护与更新。转载请注明出处(进化学习: https://www.evolutionarylearn.com/paper/ra-pythonrobotics-localization-kfl/)。

赞赏

微信赞赏支付宝赞赏

Have any Question or Comment?

2 comments on “PythonRobotics机器人算法-定位算法之扩展/无损卡尔曼滤波定位(Extended/Unscented Kalman Filter localization)

[…] 扩展卡尔曼滤波定位(Extended Kalman Filter localization) […]

[…] 卡尔曼滤波(Kalman filter)是一种高效率的递归滤波器(自回归滤波器),它能够从一系列的不完全及包含噪声的测量中,估计动态系统的状态。卡尔曼滤波会根据各测量量在不同时间下的值,考虑各时间下的联合分布,再产生对未知变数的估计,因此会以只以单一测量量为基础的估计方式要准。卡尔曼滤波得名自主要贡献者之一的鲁道夫·卡尔曼。更多信息可参见《PythonRobotics机器人算法-定位算法之扩展/无损卡尔曼滤波定位》 […]

发表评论

电子邮件地址不会被公开。 必填项已用*标注

热门主题 & 页面

分类目录

博客统计

无点击次数。