EKF SLAM学习笔记02

2 扩展卡尔曼滤波EKF

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

Localization process using Extendted Kalman Filter:EKF is

=== Predict ===

$x_{Pred}=F{x}_t+B{u}_t$

$P_{Pred}=J_F{P}_t{J}_F^T+Q$

=== Update ===

$z_{Pred}=H{x}_{Pred}$

$y=z-z_{Pred}$

$S=J_H{P}_{Pred}.J_H^T+R$

$K=P_{Pred}.J_H^T{S}^{-1}$

$x_{t+1}=x_{Pred}+Ky$

$P_{t+1}=(I-K{J}_H)P_{Pred}$

注：首先，在以上公式中，$x_{Pred}=F{x}_t+B{u}_t$的一种更普遍的表达方式是：$x_{Pred}=g(x_t,u_t)$，以表现运动模型的非线性。在此继续采用$F{x}_t+B{u}_t$矩阵相乘的形式是为了对应下面的python代码，实际上我们可以看到在代码中运动模型还是非线性的（原因是$B$矩阵包含了状态量）。同样地，$z_{Pred}=H{x}_{Pred}$ 的一种更普遍的表达方式是 $z_{Pred}=h(x_{Pred})$，在此采用矩阵相乘形式是为了对应下面的python代码。

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

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

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

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

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

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

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

滤波器设计

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

$${\text{x}}_t=[x_t,y_t,{\varphi }_t,v_t]$$

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

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

$P_t$ 代表状态的协方差矩阵，

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

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

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

$${\text{u}}_t=[v_t,{\omega }_t]$$

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

$${\text{z}}_t=[x_t,y_t]$$

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

在源代码中, “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

运动模型

机器人的运动模型为：

$$\dot{x}=vcos(\varphi )$$

$$\dot{y}=vsin((\varphi )$$

$$\dot{\varphi }=\omega$$

运动模型方程：

$${\text{x}}_{t+1}=F{\text{x}}_t+B{\text{u}}_t$$

其中，

$\begin{array}{r}F=\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& 1& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 0\end{array}\right]\end{array}$

$\begin{array}{r}B=\left[\begin{array}{cc}cos(\varphi )dt& 0\\ sin(\varphi )dt& 0\\ 0& dt\\ 1& 0\end{array}\right]\end{array}$

$dt$ 是时间步长。

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

这部分的源代码： code

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

$\begin{array}{r}{J}_F=\left[\begin{array}{cccc}\frac{dx}{dx}& \frac{dx}{dy}& \frac{dx}{d\varphi }& \frac{dx}{dv}\\ \frac{dy}{dx}& \frac{dy}{dy}& \frac{dy}{d\varphi }& \frac{dy}{dv}\\ \frac{d\varphi }{dx}& \frac{d\varphi }{dy}& \frac{d\varphi }{d\varphi }& \frac{d\varphi }{dv}\\ \frac{dv}{dx}& \frac{dv}{dy}& \frac{dv}{d\varphi }& \frac{dv}{dv}\end{array}\right]\end{array}$

$\begin{array}{r}=\left[\begin{array}{cccc}1& 0& -vsin(\varphi )dt& cos(\varphi )dt\\ 0& 1& vcos(\varphi )dt& sin(\varphi )dt\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]\end{array}$

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的观测模型是：

$${\text{z}}_t=H{\text{x}}_t$$

其中

$\begin{array}{r}H=\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& 1& 0& 0\end{array}\right]\end{array}$

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

对应的雅克比矩阵：

$\begin{array}{r}{J}_H=\left[\begin{array}{cccc}\frac{dx}{dx}& \frac{dx}{dy}& \frac{dx}{d\varphi }& \frac{dx}{dv}\\ \frac{dy}{dx}& \frac{dy}{dy}& \frac{dy}{d\varphi }& \frac{dy}{dv}\end{array}\right]\end{array}$

$\begin{array}{r}=\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& 1& 0& 0\end{array}\right]\end{array}$

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