PythonRobotics-擴展卡爾曼定位

原文鏈接

定位

擴展卡爾曼濾波定位（Extended Kalman Filter Localization）

藍色的是真實軌跡
黑色的里程計推演軌跡
綠色的點是GPS數據，

==>>

紅色線是EKF推演推演路徑
紅色圓圈是估計斜方差

擴展卡爾曼基本原理

參考鏈接

濾波器設計

仿真設計初始時間 t 時刻, 機器人狀態如下
$X_t=[x_t,y_t,\phi_t,v_t]$
x,y 是2D位置，$\phi$ 是方向，v 是速度

名稱	含義
x,y	2D位置
$\phi$	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]$

運動學模型建立

機器人模型
$\dot x = v*cos(\phi) \\ \dot y= v* sin(\phi) \\ \dot \phi = \omega$
運動學模型應該是
$x_{t+1}=Fx_t+Bu_t$

預測－２個公式

x_t 當前時刻狀態比如(x,y)　u_t 控制量輸入　比如機器人速度 v
狀態預測公式
$x_{pred}=Fx_t+Bu_t$
方差預測公式 　Q: 運動過程噪聲
$P_{pred}=J_F P_t J_F^T+Q$
$x_t$ : {x,y,yaw,v}
$u_t$ : {v, w}
根據上文數學公式 dt是時間週期 假定機器人前向速度不會改變,那麼當前速度就是等於歷史速度
里程計推演公式
$\begin{bmatrix} x_{pred} \\ y_{pred} \\ \phi_{pred} \\ v_{pred} \end{bmatrix}= \begin{bmatrix} x_{t}+​v*cos(ϕ)dt \\ y_{t}+v*sin(ϕ)dt \\ \phi_{t}+w*dt \\ v_{t} \end{bmatrix}$
==>>
$\begin{bmatrix} x_{pred} \\ y_{pred} \\ \phi_{pred} \\ v_{pred} \end{bmatrix} =\begin{bmatrix} 1&0&0&0 \\ 0&1&0&0 \\ 0&0&1&0 \\ 0&0&0&0 \end{bmatrix} \begin{bmatrix} x_{t} \\ y_{t} \\ \phi_{t} \\ v_{t} \end{bmatrix} +\begin{bmatrix} cos(\phi)dt&0 \\ sin(\phi)dt&0 \\ 0&dt \\ 1&0 \end{bmatrix} \begin{bmatrix} v_t \\ w \end{bmatrix}$

雅克比矩陣公式如下

$J= \begin{bmatrix} \frac{\partial y1 }{\partial x1} &...& \frac{\partial y1 }{\partial xn} \\ ...& ... & ... \\ \frac{\partial yn }{\partial x1} &...& \frac{\partial yn }{\partial xn} \\ \end{bmatrix}$
這裏方差的J_F爲

$J_F= \begin{bmatrix} \frac{\partial x_{pred} }{\partial x_t} & \frac{\partial x_{pred} }{\partial y_t} & \frac{\partial x_{pred} }{\partial \phi_t} & \frac{\partial x_{pred} }{\partial v_t} \\ \frac{\partial y_{pred} }{\partial x_t} & \frac{\partial xy{pred} }{\partial y_t} & \frac{\partial y_{pred} }{\partial \phi_t} & \frac{\partial y_{pred} }{\partial v_t} \\ \frac{\partial \phi_{pred} }{\partial x_t} & \frac{\partial \phi_{pred} }{\partial y_t} & \frac{\partial \phi_{pred} }{\partial \phi_t} & \frac{\partial \phi_{pred} }{\partial v_t} \\ \frac{\partial v_{pred} }{\partial x_t} & \frac{\partial v_{pred} }{\partial y_t} & \frac{\partial v_{pred} }{\partial \phi_t} & \frac{\partial v_{pred} }{\partial v_t} \\ \end{bmatrix}$
=>>
雅克比矩陣的意思就是求偏導函數,對某個變量求偏導時，將其他變量設定爲定值
$\frac{\partial x_{pred} }{\partial x_t}$ 就是對
$x_{t}+​v*cos(ϕ)dt$ 求偏導數　x_t 的係數爲1 則偏導爲１
$\frac{\partial x_{pred} }{\partial \phi_t}$ 就是對
$x_{t}+​v*cos(ϕ)dt$ 求偏導數　$\phi$ 的係數爲 vcos($\phi$) 則偏導爲 -vsin($\phi$)
$J_F= \begin{bmatrix} 1 & 0 & -vsin(\phi)dt & cos(\phi)dt \\ 0&1&vcos(\phi)dt&sin(\phi)dt \\ 0&0&1&0 \\ 0&0&0&1 \end{bmatrix}$
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}=Hx_{pred}$
因爲我們的觀測只有GPS,x 和y的位置
因此
$\begin{bmatrix} Zx_{pred} \\ Zy_{pred} \end{bmatrix} =\begin{bmatrix} 1&0&0&0 \\ 0&1&0&0 \end{bmatrix} \begin{bmatrix} x_{pred} \\ y_{pred} \\ \phi_{pred} \\ v_{pred} \end{bmatrix}$
$H =\begin{bmatrix} 1&0&0&0 \\ 0&1&0&0 \end{bmatrix} \\ J_H =\begin{bmatrix} 1&0&0&0 \\ 0&1&0&0 \end{bmatrix}$
觀測與運動推測的
誤差= 觀測值　- 預測值
$error=z-z_{pred}$
求卡爾曼增益 記住此公式即可
$S=J_HP_{pred}J_H^T+R \\ K=P_{pred}J_H^TS^{-1}$
更新新的位置和方差
$x_{t+1} = x_{pred}+K*error \\ P_{t+1}=(I-KJ_H)P_{pred}$
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))
    # 真實位置狀態矩陣　４行