PythonRobotics-扩展卡尔曼定位

原文链接

定位

扩展卡尔曼滤波定位（Extended Kalman Filter Localization）

蓝色的是真实轨迹
黑色的里程计推演轨迹
绿色的点是GPS数据，

==>>

红色线是EKF推演推演路径
红色圆圈是估计斜方差

扩展卡尔曼基本原理

参考链接

滤波器设计

仿真设计初始时间 t 时刻, 机器人状态如下
$$X_t=[x_t,y_t,{\varphi }_t,v_t]$$
x,y 是2D位置，$\varphi$ 是方向，v 是速度

名称	含义
x,y	2D位置
$\varphi$	2D方向
v	速度

代码中含义

名称	含义
xEst	状态向量（state vector）
$P_t$	状态协方差矩阵（covariace matrix of the state）
$Q$	噪声协方差矩阵（covariance matrix of process noise）
$R$	t 时刻观测噪声协方差矩阵（covariance matrix of observation noise at time 𝑡）

机器人有速度传感器和陀螺仪传感器，所以每个周期输入向量

$$u_t=[v_t,{\omega }_t]$$
机器人有GNSS 或者GPS传感器，每个周期机器人观测位置如下
$$z_t=[x_t,y_t]$$

运动学模型建立

机器人模型
$$\begin{gathered} \dot{x}=v\ast cos(\varphi ) \\ \dot{y}=v\ast sin(\varphi ) \\ \dot{\varphi }=\omega \end{gathered}$$
运动学模型应该是
$$x_{t+1}=F{x}_t+B{u}_t$$

预测－２个公式

x_t 当前时刻状态比如(x,y)　u_t 控制量输入　比如机器人速度 v
状态预测公式
$$x_{pred}=F{x}_t+B{u}_t$$
方差预测公式 　Q: 运动过程噪声
$$P_{pred}=J_F{P}_t{J}_F^T+Q$$
$x_t$ : {x,y,yaw,v}
$u_t$ : {v, w}
根据上文数学公式 dt是时间周期 假定机器人前向速度不会改变,那么当前速度就是等于历史速度
里程计推演公式
$$\left[\begin{array}{c}{x}_{pred}\\ y_{pred}\\ {\varphi }_{pred}\\ v_{pred}\end{array}\right]=\left[\begin{array}{c}{x}_t+v\ast cos(\varphi )dt\\ y_t+v\ast sin(\varphi )dt\\ {\varphi }_t+w\ast dt\\ v_t\end{array}\right]$$
==>>
$$\left[\begin{array}{c}{x}_{pred}\\ y_{pred}\\ {\varphi }_{pred}\\ v_{pred}\end{array}\right]=\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& 1& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 0\end{array}\right]\left[\begin{array}{c}{x}_t\\ y_t\\ {\varphi }_t\\ v_t\end{array}\right]+\left[\begin{array}{cc}cos(\varphi )dt& 0\\ sin(\varphi )dt& 0\\ 0& dt\\ 1& 0\end{array}\right]\left[\begin{array}{c}{v}_t\\ w\end{array}\right]$$

雅克比矩阵公式如下

$$J=\left[\begin{array}{ccc}\frac{\partial y1}{\partial x1}& ...& \frac{\partial y1}{\partial xn}\\ ...& ...& ...\\ \frac{\partial yn}{\partial x1}& ...& \frac{\partial yn}{\partial xn}\end{array}\right]$$
这里方差的J_F为

$$J_F=\left[\begin{array}{cccc}\frac{\partial x_{pred}}{\partial x_t}& \frac{\partial x_{pred}}{\partial y_t}& \frac{\partial x_{pred}}{\partial {\varphi }_t}& \frac{\partial x_{pred}}{\partial v_t}\\ \frac{\partial y_{pred}}{\partial x_t}& \frac{\partial xypred}{\partial y_t}& \frac{\partial y_{pred}}{\partial {\varphi }_t}& \frac{\partial y_{pred}}{\partial v_t}\\ \frac{\partial {\varphi }_{pred}}{\partial x_t}& \frac{\partial {\varphi }_{pred}}{\partial y_t}& \frac{\partial {\varphi }_{pred}}{\partial {\varphi }_t}& \frac{\partial {\varphi }_{pred}}{\partial v_t}\\ \frac{\partial v_{pred}}{\partial x_t}& \frac{\partial v_{pred}}{\partial y_t}& \frac{\partial v_{pred}}{\partial {\varphi }_t}& \frac{\partial v_{pred}}{\partial v_t}\end{array}\right]$$
=>>
雅克比矩阵的意思就是求偏导函数,对某个变量求偏导时，将其他变量设定为定值
$\frac{\partial x_{pred}}{\partial x_t}$ 就是对
$x_t+v\ast cos(\varphi )dt$ 求偏导数　x_t 的系数为1 则偏导为１
$\frac{\partial x_{pred}}{\partial {\varphi }_t}$ 就是对
$x_t+v\ast cos(\varphi )dt$ 求偏导数　$\varphi$ 的系数为 vcos($\varphi$) 则偏导为 -vsin($\varphi$)
$$J_F=\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]$$
Q为运动噪声,设定Q为

Q=np.diag([0.1, 0.1, np.deg2rad(1.0), 1.0])**2  # predict state covariance
Q=array([[  1.00000000e-02,   0.00000000e+00,   0.00000000e+00,
          0.00000000e+00],
       [  0.00000000e+00,   1.00000000e-02,   0.00000000e+00,
          0.00000000e+00],
       [  0.00000000e+00,   0.00000000e+00,   3.04617420e-04,
          0.00000000e+00],
       [  0.00000000e+00,   0.00000000e+00,   0.00000000e+00,
          1.00000000e+00]])

更新

–６个公式
求观测函数H
$$z_{pred}=H{x}_{pred}$$
因为我们的观测只有GPS,x 和y的位置
因此
$$\left[\begin{array}{c}Z{x}_{pred}\\ Z{y}_{pred}\end{array}\right]=\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& 1& 0& 0\end{array}\right]\left[\begin{array}{c}{x}_{pred}\\ y_{pred}\\ {\varphi }_{pred}\\ v_{pred}\end{array}\right]$$
$$\begin{gathered} H=\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& 1& 0& 0\end{array}\right] \\ {J}_H=\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& 1& 0& 0\end{array}\right] \end{gathered}$$
观测与运动推测的
误差= 观测值　- 预测值
$$error=z-z_{pred}$$
求卡尔曼增益 记住此公式即可
$$\begin{gathered} S=J_H{P}_{pred}{J}_H^T+R \\ K=P_{pred}{J}_H^T{S}^{-1} \end{gathered}$$
更新新的位置和方差
$$\begin{gathered} x_{t+1}=x_{pred}+K\ast error \\ {P}_{t+1}=(I-K{J}_H)P_{pred} \end{gathered}$$
R为观测方差
此处记录为

R = np.diag([1.0, 1.0])**2  # Observation x,y position covariance

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

"""
Extended kalman filter (EKF) localization sample
author: Atsushi Sakai (@Atsushi_twi)
"""

import numpy as np
import math
import matplotlib.pyplot as plt

# Estimation parameter of EKF diag 生成以对角线为数值生成矩阵　deg2rad角度转弧度
Q = np.diag([1.0, 1.0])**2  # Observation x,y position covariance
R = np.diag([0.1, 0.1, np.deg2rad(1.0), 1.0])**2  # predict state covariance

#  Simulation parameter
Qsim = np.diag([0.5, 0.5])**2
Rsim = np.diag([1.0, np.deg2rad(30.0)])**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]]).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]
    #在准确位置上加入一定噪声仿真GPS数据
    z = np.array([[zx, zy]])

    # 仿真实际的速度反馈，实际的速度加入一定噪声
    ud1 = u[0, 0] + np.random.randn() * Rsim[0, 0]
    ud2 = u[1, 0] + np.random.randn() * Rsim[1, 1]
    ud = np.array([[ud1, ud2]]).T

　　 #获得航迹
    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]), 0],
                  [DT * math.sin(x[2, 0]), 0],
                  [0.0, DT],
                  [1.0, 0.0]])

    x = F.dot(x) + B.dot(u)

    return x


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

    z = H.dot(x)

    return z


def jacobF(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 jacobH(x):
    # Jacobian of Observation Model
    jH = np.array([
        [1, 0, 0, 0],
        [0, 1, 0, 0]
    ])

    return jH


def ekf_estimation(xEst, PEst, z, u):

    """
    假设当前推演值是x_t 观测值是z_t　这了里程计是推测值，GPS是观测值
    那么ekf_x估计位置是　
    ekf_t= x_t+ k(z_t-x_t) 
    ekf滤波值=里程计值＋k*(GPS位置－里程计位置)
    ekf 主要目的是为了求k值
    假如k=0 那么ekf估计值就是推测值
    假如k=1 那么ekf估计值就是观测值
    
    """
    #  Predict　预测位置
    xPred = motion_model(xEst, u)
    # 得到运动学模型雅克比矩阵
    jF = jacobF(xPred, u)
    # R预测值的协方差
    PPred = jF.dot(PEst).dot(jF.T) + R

    #  Update
    #得到观测位置
    zPred = observation_model(xPred)
    
    jH = jacobH(xPred)
    y = z.T - zPred
    S = jH.dot(PPred).dot(jH.T) + Q
    K = PPred.dot(jH.T).dot(np.linalg.inv(S))
    xEst = xPred + K.dot(y)
    PEst = (np.eye(len(xEst)) - K.dot(jH)).dot(PPred)

    return xEst, PEst


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.array([[math.cos(angle), math.sin(angle)],
                  [-math.sin(angle), math.cos(angle)]])
    fx = R.dot(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))
    # 真实位置状态矩阵　４行１列
    xTrue = np.zeros((4, 1))
    #协方差矩阵
    PEst = np.eye(4)
	# 里程计
    xDR = 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()
　　　　　# 输入上次的准确位置　里程计数据　和速度
　　　　　# 返回得到模拟真实轨迹　不准确的GPS数据　里程计数据　不准确的速度向量　
        xTrue, z, xDR, ud = observation(xTrue, xDR, u)
　　　　　# 输入推算位置　协方差　带噪声的GPS位置　带噪声的速度输入
        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(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)
def main():
    print(__file__ + " start!!")

    time = 0.0

    # State Vector [x y yaw v]' 估计位置状态矩阵　４行１列
    xEst = np.zeros((4, 1))
    # 真实位置状态矩阵　４行