卡尔曼滤波与扩展卡尔曼滤波实现

参考
How a Kalman filter works, in pictures
python Robotics
知乎-卡尔曼滤波

代码

%matplotlib
import math
import matplotlib.pyplot as plt
import numpy as np
from scipy.spatial.transform import Rotation as Rot

机器人有4个状态量，坐标x，坐标y，航向角yaw，速度v，记录为[x, y, yaw, v]
Q是没有被跟踪的干扰，通过协方差表示为噪声

$$
\boldsymbol{Q}=
\left[\begin{array}{c}
0.01 & 0 & 0 & 0 \\
0 & 0.01 & 0 & 0 \\
0 & 0 & 0.003 & 0 \\
0 & 0 & 0 & 1 \\
\end{array}\right] \quad
$$

R是传感器的不确定性，通过协方差表示为噪声
$$
\boldsymbol{R}=
\left[\begin{array}{c}
1 & 0 \\
0 & 4 \\
\end{array}\right] \quad
$$
速度传感器与陀螺仪的输入误差
$$
\boldsymbol{INPUT_{noise}}=
\left[\begin{array}{c}
1 & 0 \\
0 & 0.27 \\
\end{array}\right] \quad
$$
GPS传感器误差
$$
\boldsymbol{GPS_{noise}}=
\left[\begin{array}{c}
0.25 & 0 \\
0 & 0.25 \\
\end{array}\right] \quad
$$

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, 2.0]) ** 2  # Observation x,y position covariance

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

机器人有一个速度传感器和一个陀螺仪传感器。所以该机器人的输入数据为速度m/s，角速度rad/s
$$
\boldsymbol{u}=
\left[\begin{array}{c}
v \\
ω \\
\end{array}\right] \quad
$$

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

这里是给定一个初始值，并加入随机噪声，让其不断的进行更新，从而对xTrue进行追踪。

def observation(xTrue, xd, u):
    xTrue = motion_model(xTrue, u)  # x' = F @ x + B @ u

    # add noise to gps x-y
    # H @ x + GPS noise
    z = observation_model(xTrue) + GPS_NOISE @ np.random.randn(2, 1)

    # add noise to input
    ud = u + INPUT_NOISE @ np.random.randn(2, 1)    # input vector + input noise

    xd = motion_model(xd, ud)

    return xTrue, z, xd, ud

这里需要了解一下协方差矩阵，相同状态量之间有关系，不同状态量之间就没有关系

下式中，a是1与1的一个关系，g是2与3的一个关系

4个状态量的话，就是4x4的矩阵

$$
\left[\begin{array}{c}
& 1 & 2 & 3\\
1 & a & b & c\\
2 & e & f & g\\
3 & h & i & j\\
\end{array}\right] \quad
$$

F为状态转换方程，将t时刻的状态转换至t+1时刻的状态; 代表的是当前状态量和未来状态量之间的关系，上面解释了为什么对角线之外为0
B是外部控制矩阵，将运动量测量值u的作用映射到状态向量上
u是运动测量值，如加速度，转向角等等（对于没有外部控制的简单系统来说，这部分可以忽略）

$$
x_{t+1} = F x_t + B u \\
P_{t+1} = F P_t F^{-1} \\
$$
$$
\boldsymbol{F}=
\left[\begin{array}{c}
1 & 0 & 0 & 0 \\
0 & 1 & 0 & 0 \\
0 & 0 & 1 & 0 \\
0 & 0 & 0 & 0 \\
\end{array}\right] \quad
\boldsymbol{B}=
\left[\begin{array}{c}
\cos(\varphi)\Delta t & 0 \\
\sin(\varphi)\Delta t & 0 \\
0 & \Delta t \\
1 & 0 \\
\end{array}\right] \quad
$$

但是这里，我们需要计算这个外部控制量。这是一个变化量，是当前状态量的变化量。

当前状态量有坐标x，坐标y，角速度，速度，所以变化量可以写为
$$
\cos(\varphi)\Delta t \cdot v_t \\
\sin(\varphi)\Delta t \cdot v_t \\
ω \cdot \Delta t \\
v_t\\
可以通过矩阵相乘计算获得=
\left[\begin{array}{c}
\cos(\varphi)\Delta t & 0 \\
\sin(\varphi)\Delta t & 0 \\
0 & \Delta t \\
1 & 0 \\
\end{array}\right] \quad
\cdot
\left[\begin{array}{c}
v \\
ω \\
\end{array}\right] \quad
$$

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

    # [2, 0] is the index of list x. means yaw value
    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

H为转换矩阵。它将状态向量映射到测量值所在的空间中，代表的是GPS的传感器数据相关性，乘以现在的位置信息就是通过预测得到的新的位置信息

因为GPS只能获得位置信息，所以x，y以外都为0

def observation_model(x):
    H = np.array([
        [1, 0, 0, 0],
        [0, 1, 0, 0]
    ])

    z = H @ x

    return z

在卡尔曼滤波器中，位置信息成线性变化
$$
x_{t+1} = x_t + v cos(yaw) \Delta t \\
y_{t+1} = y_t + v sin(yaw) \Delta t \\
yaw_{t+1} = yaw_t + ω \Delta t \\
v_{t+1} = v_t \\
$$
$$
\boldsymbol{F}=
\left[\begin{array}{c}
1 & 0 & 0 & \Delta t \cos(\varphi) \\
0 & 1 & 0 & \Delta t \sin(\varphi) \\
0 & 0 & 1 & 0 \\
0 & 0 & 0 & 1 \\
\end{array}\right] \quad
$$

def calc_F(x, u):
    yaw = x[2, 0]
    v = u[0, 0]

    F = np.array([
        [1.0, 0.0, 0.0, DT * math.cos(yaw)],
        [0.0, 1.0, 0.0, DT * math.sin(yaw)],
        [0.0, 0.0, 1.0, 0.0],
        [0.0, 0.0, 0.0, 1.0]])
    return F

在扩展卡尔曼滤波器中，位置信息成非线性变化，通过一次微分，表示为x与yaw的关系，x与v的关系，y与yaw的关系，y与v的关系

Jacobian记为
$$
\frac{dx}{dyaw} = -v sin(yaw) \Delta t \\
\frac{dx}{dv} = cos(yaw) \Delta t \\
\frac{dy}{dyaw} = v cos(yaw) \Delta t \\
\frac{dy}{dv} = sin(yaw) \Delta t \\
$$
$$
\boldsymbol{Jacobian_f}=
\left[\begin{array}{c}
1 & 0 & -v\sin(\varphi)\Delta t & \cos(\varphi)\Delta t \\
0 & 1 & v\cos(\varphi)\Delta t & \sin(\varphi)\Delta t \\
0 & 0 & 1 & 0 \\
0 & 0 & 0 & 1 \\
\end{array}\right] \quad
$$

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)    # the relation of x and yaw
    dx/dv = dt*cos(yaw)         # the relation of x and v
    dy/dyaw = v*dt*cos(yaw)     # the relation of y and yaw
    dy/dv = dt*sin(yaw)         # the relation of y and v
    """
    yaw = x[2, 0]     # [x, y, yaw, v]
    v = u[0, 0]       # the speed info from sensor
    jF = np.array([   # the nonlinear relation of F write as jF
        [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

协方差H的Jacobian不发生变化
$$
\boldsymbol{Jacobian_h}=
\left[\begin{array}{c}
1 & 0 & 0 & 0 \\
0 & 1 & 0 & 0 \\
\end{array}\right] \quad
$$

def jacob_h():
    # Jacobian of Observation Model
    jH = np.array([
        [1, 0, 0, 0],
        [0, 1, 0, 0]
    ])

    return jH

扩展卡尔曼滤波预测流程
Predict
$$
x_{t+1} = Fx_t + Bu \\
P_{t+1} = J_fP_tJ_f^T + Q\\
$$
Update
$$
z_{t+1} = Hx_{t+1} \\
y = z_t - z_{t+1} \\
S=J_hP_{t+1}J_h^T + R \\
K=P_{t+1}J_h^TS^{-1} \\
x_{t+1} = x_t + Ky \\
P_{t+1} = (1 - KJ_h)P_{t+1}
$$

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

卡尔曼滤波预测流程
Predict
$$
x_{t+1} = Fx_t + Bu \\
P_{t+1} = FP_tF^T + Q\\
$$
Update
$$
z_{t+1} = Hx_{t+1} \\
y = z_t - z_{t+1} \\
S=HP_{t+1}H^T + R \\
K=P_{t+1}H^TS^{-1} \\
x_{t+1} = x_t + Ky \\
P_{t+1} = (1 - KH)P_{t+1}
$$

def kf_estimation(xEst, PEst, z, u):
    #  Predict
    xPred = motion_model(xEst, u)
    F = calc_F(xPred, u)
    PPred = F @ PEst @ F.T + Q

    #  Update
    H = jacob_h()
    zPred = observation_model(xPred)
    y = z - zPred
    S = H @ PPred @ H.T + R
    K = PPred @ H.T @ np.linalg.inv(S)
    xEst = xPred + K @ y
    PEst = (np.eye(len(xEst)) - K @ H) @ PPred
    return xEst, PEst

封装了一个画图功能，用于画椭圆形

def plot_covariance_ellipse(xEst, PEst, color):  # 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[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, color)

主函数
红色线为扩展卡尔曼滤波，黄色线为卡尔曼滤波

def main():

    time = 0.0

    # State Vector [x y yaw v]'
    xEst = np.zeros((4, 1))  # t-1
    xTrue = np.zeros((4, 1))
    PEst = np.eye(4)

    ekf_Est = np.zeros((4, 1))
    ekf_PEst = np.eye(4)
    kf_Est = np.zeros((4, 1))
    kf_PEst = np.eye(4)

    xDR = np.zeros((4, 1))  # Dead reckoning

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

    while SIM_TIME >= time:
        time += DT
        u = sensor_input()

        xTrue, z, xDR, ud = observation(xTrue, xDR, u)

        ekf_Est, ekf_PEst = ekf_estimation(ekf_Est, ekf_PEst, z, ud)
        kf_Est, kf_PEst = kf_estimation(kf_Est, kf_PEst, z, ud)
        # store data history
        hxEst = np.hstack((hxEst, ekf_Est))
        kf_hxEst = np.hstack((kf_hxEst, kf_Est))
        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")
            plt.plot(kf_hxEst[0, :].flatten(),
                     kf_hxEst[1, :].flatten(), "-y")
            plot_covariance_ellipse(ekf_Est, ekf_PEst, "--r")
            plot_covariance_ellipse(kf_Est, kf_PEst, "--y")
            plt.axis("equal")
            plt.grid(True)
            plt.pause(0.001)
            plt.clf()
    plt.close()


if __name__ == '__main__':
    main()