Unscented Kalman filter无迹卡尔曼滤波

扩展卡尔曼滤波(EKF)用一阶导数来对非线性模型做局部线性化,而无迹卡尔曼滤波UKF使用所谓“统计线性化”方法,从一个已知的分布中做采样,也就是“生成sigma points”,来“模拟”非线性模型的随机分布。UKF也包含经典的“预测”和“更新”过程,只不过多了一步sigma points的采样。

参考:

  • SLAM笔记六——Unscented Kalman Filter
  • unscented_kalman_filter.py代码
  • Unscented Kalman Filter Tutorial

我们将通过参考链接中的python代码简介无迹卡尔曼滤波的计算过程。

计算Unscented Transform参数

Unscented Transform是无迹卡尔曼滤波的参数,指的是采样sigma points的已知分布的一些参数:

Unscented Kalman filter无迹卡尔曼滤波_第1张图片

计算权重weights

Unscented Kalman filter无迹卡尔曼滤波_第2张图片
这部分的代码为:


#  UKF Parameter 无迹卡尔曼滤波参数设定值:
ALPHA = 0.001
BETA = 2
KAPPA = 0

def setup_ukf(nx):
    lamb = ALPHA ** 2 * (nx + KAPPA) - nx
    # calculate weights
    wm = [lamb / (lamb + nx)]
    wc = [(lamb / (lamb + nx)) + (1 - ALPHA ** 2 + BETA)]
    for i in range(2 * nx):
        wm.append(1.0 / (2 * (nx + lamb)))
        wc.append(1.0 / (2 * (nx + lamb)))
    gamma = math.sqrt(nx + lamb)

    wm = np.array([wm])
    wc = np.array([wc])

    return wm, wc, gamma

生成sigma points

Unscented Kalman filter无迹卡尔曼滤波_第3张图片
这部分代码为:

def generate_sigma_points(xEst, PEst, gamma):
    '''
    sigma点的生成:
    X_0 = 均值
    X_i = 均值 + (sqrt((nx+lamb)协方差))_i, for i=1,2,...,n
    X_i = 均值 - (sqrt((nx+lamb)协方差))_i-n, for i=n+1,n+2,...,2n
    
    返回的sigma矩阵形式举例:
    [[ 0.     0.002  0.     0.     0.    -0.002  0.     0.     0.   ]
     [ 0.     0.     0.002  0.     0.     0.    -0.002  0.     0.   ]
     [ 0.     0.     0.     0.002  0.     0.     0.    -0.002  0.   ]
     [ 0.     0.     0.     0.     0.002  0.     0.     0.    -0.002]]
    '''
    # X_0是均值
    sigma = xEst
    # 协方差矩阵的开平方:
    Psqrt = scipy.linalg.sqrtm(PEst)
    n = len(xEst[:, 0])
    # Positive direction
    for i in range(n):
        sigma = np.hstack((sigma, xEst + gamma * Psqrt[:, i:i + 1]))
    # Negative direction
    for i in range(n):
        sigma = np.hstack((sigma, xEst - gamma * Psqrt[:, i:i + 1]))
    return sigma

UKF过程

预测:

Unscented Kalman filter无迹卡尔曼滤波_第4张图片
更新:

Unscented Kalman filter无迹卡尔曼滤波_第5张图片
这一过程的代码在:

def ukf_estimation(xEst, PEst, z, u, wm, wc, gamma):
    '''
    z: 带噪声的观测
    u: 带噪声的输入
    wm, wc, gamma: 无迹变换参数
    '''
    #  Predict 预测
    # 生成sigma采样点:
    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 - zPred
    # 对上一步计算过的均值的估计再次生成sigma采样点:
    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

但需要注意的是,以上代码的“更新”部分与上面的6-14式不能完全一一对应,但因为在这个python实例中, h ( ) h() h()观测模型是简单的x、y位置输出,所以实际上和6-14式的计算是一样的。

为便于理解,我把这部分代码做了调整:

def ukf_estimation(xEst, PEst, z, u, wm, wc, gamma):
    '''
    z: 带噪声的观测
    u: 带噪声的输入
    wm, wc, gamma: 无迹变换参数
    '''
    #  Predict 预测
    # 生成sigma采样点: 2:
    sigma = generate_sigma_points(xEst, PEst, gamma)
    # 运动模型预测: 3:
    sigma = predict_sigma_motion(sigma, u)
    # 将采样点与权重相乘求对均值的估计: 4:
    xPred = (wm @ sigma.T).T
    # 求协方差的估计: 5: 
    PPred = calc_sigma_covariance(xPred, sigma, wc, Q)

    #  Update 更新
    # 对上一步计算过的均值的估计再次生成sigma采样点: 6:
    sigma = generate_sigma_points(xPred, PPred, gamma)
    # 用生成的sigma采样点做观测预测: 7:
    z_sigma = predict_sigma_observation(sigma)
    # 求均值:  8:
    zPred = (wm @ z_sigma.T).T
    # 求卡尔曼增益计算中的S:   9:
    st = calc_sigma_covariance(zPred, z_sigma, wc, R)
    # 求xz间协方差:   10: 
    Pxz = calc_pxz(sigma, xPred, z_sigma, zPred, wc)
    # 计算卡尔曼增益:  11:
    K = Pxz @ np.linalg.inv(st)
    y = z - zPred
    # 更新均值:  12: 
    xEst = xPred + K @ y
    # 更新协方差:  13: 
    PEst = PPred - K @ st @ K.T

    return xEst, PEst

最后附上带有注释的、调整后的完整python代码:

"""

Unscented kalman filter (UKF) localization sample

author: Atsushi Sakai (@Atsushi_twi)

modification: Junchuan Zhang

"""

import math

import matplotlib.pyplot as plt
import numpy as np
import scipy.linalg

# Covariance for UKF 协方差矩阵
# 运动过程协方差:
Q = np.diag([
    0.1,  # variance of location on x-axis
    0.1,  # variance of location on y-axis
    np.deg2rad(1.0),  # variance of yaw angle
    1.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 # GPS值噪声

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

#  UKF Parameter 无迹卡尔曼滤波参数设定值:
ALPHA = 0.001
BETA = 2
KAPPA = 0

show_animation = True


def calc_input():
    v = 1.0  # [m/s]
    yawRate = 0.1  # [rad/s]
    u = np.array([[v, yawRate]]).T
    return u


def observation(xTrue, xd, u):
    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 = np.array([[DT * math.cos(x[2]), 0],
                  [DT * math.sin(x[2]), 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 generate_sigma_points(xEst, PEst, gamma):
    '''
    sigma点的生成:
    X_0 = 均值
    X_i = 均值 + (sqrt((nx+lamb)协方差))_i, for i=1,2,...,n
    X_i = 均值 - (sqrt((nx+lamb)协方差))_i-n, for i=n+1,n+2,...,2n
    
    返回的sigma矩阵形式举例:
    [[ 0.     0.002  0.     0.     0.    -0.002  0.     0.     0.   ]
     [ 0.     0.     0.002  0.     0.     0.    -0.002  0.     0.   ]
     [ 0.     0.     0.     0.002  0.     0.     0.    -0.002  0.   ]
     [ 0.     0.     0.     0.     0.002  0.     0.     0.    -0.002]]
    '''
    # X_0是均值
    sigma = xEst
    # 协方差矩阵的开平方:
    Psqrt = scipy.linalg.sqrtm(PEst)
    n = len(xEst[:, 0])
    # Positive direction
    for i in range(n):
        sigma = np.hstack((sigma, xEst + gamma * Psqrt[:, i:i + 1]))
    # Negative direction
    for i in range(n):
        sigma = np.hstack((sigma, xEst - gamma * Psqrt[:, i:i + 1]))
    return sigma


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

    return sigma


def predict_sigma_observation(sigma):
    """
        Sigma Points prediction 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:i + 1] @ d[:, i:i + 1].T
        
    return P


def calc_pxz(sigma, x, z_sigma, zb, wc):
    nSigma = sigma.shape[1]
    dx = sigma - x
    dz = z_sigma - zb[0:2]
    P = np.zeros((dx.shape[0], dz.shape[0]))

    for i in range(nSigma):
        P = P + wc[0, i] * dx[:, i:i + 1] @ dz[:, i:i + 1].T

    return P


def ukf_estimation(xEst, PEst, z, u, wm, wc, gamma):
    '''
    z: 带噪声的观测
    u: 带噪声的输入
    wm, wc, gamma: 无迹变换参数
    '''
    #  Predict 预测
    # 生成sigma采样点: 2:
    sigma = generate_sigma_points(xEst, PEst, gamma)
    # 运动模型预测: 3:
    sigma = predict_sigma_motion(sigma, u)
    # 将采样点与权重相乘求对均值的估计: 4:
    xPred = (wm @ sigma.T).T
    # 求协方差的估计: 5: 
    PPred = calc_sigma_covariance(xPred, sigma, wc, Q)

    #  Update 更新
    # 对上一步计算过的均值的估计再次生成sigma采样点: 6:
    sigma = generate_sigma_points(xPred, PPred, gamma)
    # 用生成的sigma采样点做观测预测: 7:
    z_sigma = predict_sigma_observation(sigma)
    # 求均值:  8:
    zPred = (wm @ z_sigma.T).T
    # 求卡尔曼增益计算中的S:   9:
    st = calc_sigma_covariance(zPred, z_sigma, wc, R)
    # 求xz间协方差:   10: 
    Pxz = calc_pxz(sigma, xPred, z_sigma, zPred, wc)
    # 计算卡尔曼增益:  11:
    K = Pxz @ np.linalg.inv(st)
    y = z - zPred
    # 更新均值:  12: 
    xEst = xPred + K @ y
    # 更新协方差:  13: 
    PEst = PPred - K @ st @ K.T

    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 setup_ukf(nx):
    lamb = ALPHA ** 2 * (nx + KAPPA) - nx
    # calculate weights
    wm = [lamb / (lamb + nx)]
    wc = [(lamb / (lamb + nx)) + (1 - ALPHA ** 2 + BETA)]
    for i in range(2 * nx):
        wm.append(1.0 / (2 * (nx + lamb)))
        wc.append(1.0 / (2 * (nx + lamb)))
    gamma = math.sqrt(nx + lamb)

    wm = np.array([wm])
    wc = np.array([wc])

    return wm, wc, gamma


def main():
    print(__file__ + " start!!")

    nx = 4  # State Vector [x y yaw v]'
    xEst = np.zeros((nx, 1))
    xTrue = np.zeros((nx, 1))
    PEst = np.eye(nx)
    xDR = np.zeros((nx, 1))  # Dead reckoning
    # 初始化参数:
    wm, wc, gamma = setup_ukf(nx)

    # history
    hxEst = xEst
    hxTrue = xTrue
    hxDR = xTrue
    hz = np.zeros((2, 1))

    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.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(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()

你可能感兴趣的:(SLAM知识学习,python,经验分享,卡尔曼滤波算法)