無人駕駛汽車系統入門（二）——高級運動模型和擴展卡爾曼濾波

前言：上一篇文章的最後我們提到卡爾曼濾波存在着一個非常大的侷限性——它僅能對線性的處理模型和測量模型進行精確的估計，在非線性的場景中並不能達到最優的估計效果。所以之前爲了保證我們的處理模型是線性的，我們上一節中使用了恆定速度模型，然後將估計目標的加減速用處理噪聲來表示，這一模型用來估算行人的狀態其實已經足夠了，但是在現實的駕駛環境中，我們不僅要估計行人，我們除了估計行人狀態以外，我們還需要估計其他車輛，自行車等等狀態，他們的狀態估計顯然不能使用簡單的線性系統來描述，這裏，我們介紹非線性系統情況下的一種廣泛使用的濾波算法——擴展卡爾曼濾波(Extended Kalman Filter, EKF)。

本節主要講解非線性系統中廣泛使用的擴展卡爾曼濾波算法，我們通常將該算法應用於實際的車輛狀態估計（或者說車輛追蹤）中。另外，實際的車輛追蹤運動模型顯然不能使用簡單的恆定速度模型來建模，在本節中會介紹幾種應用於車輛追蹤的高級運動模型。並且已經其中的CTRV模型來構造我們的擴展卡爾曼濾波。最後，在代碼實例中，我會介紹如何使用EKF做多傳感器融合。

編寫不易，轉載請附來源：http://blog.csdn.net/adamshan/article/details/78265754

應用於車輛追蹤的高級運動模型

首先要明確的一點是，不管是什麼運動模型，本質上都是爲了幫助我們簡化問題，所以我們可以根據運動模型的複雜程度（次數）來給我們常用的運動模型分一下類。
一次運動模型（也別稱爲線性運動模型）：

恆定速度模型（Constant Velocity, CV）
恆定加速度模型（Constant Acceleration, CA）

這些線性運動模型假定目標是直線運動的，並不考慮物體的轉彎。

二次運動模型：

恆定轉率和速度模型（Constant Turn Rate and Velocity，CTRV）
恆定轉率和加速度模型（Constant Turn Rate and Acceleration，CTRA）

CTRV目前多用於機載追蹤系統（飛機），這些二次運動模型大多假定速度 $v$ 和偏航角速度（yaw rate） $ω$ 沒有關係，因此，在這類運動模型中，由於偏航角速度測量的擾動（不穩定），即使車輛沒有移動，我們的運動模型下的角速度也會發生細微的變化。

爲了解決這個問題，速度 $v$ 和偏航角速度 $ω$ 的關聯可以通過設定轉向角 $Φ$ 恆定來建立，這樣就引出了恆定轉向角和速度模型（Constant Steering Angle and Velocity，CSAV）, 另外，速度可以別假定爲線性變化的，進而引出了常曲率和加速度模型（Constant Curvature and Acceleration，CCA）。

這些運動模型的關係如圖：

運動模型的狀態轉移公式

由於除CCA以外，以上的運動模型都非常著名，故本文不提供詳細的推導過程。本文提供CV和CTRV模型的狀態轉移公式。

狀態轉移公式：就是我們的處理模型由上一狀態的估計計算下一個狀態的先驗分佈的計算公式，可以理解爲我們基於一定的先驗知識總結出來的運動公式。

CV模型：
CV模型的狀態空間可以表示爲：

$\vec{x} (t) = (x y v_{x} v_{y})^{T}$
那麼轉移函數爲：
$\vec{x} (t + Δ t) = (\begin{matrix} x (t) + Δ t v_{x} \\ y (t) + Δ t v_{y} \\ v_{x} \\ v_{y} \end{matrix})$
CTRV模型：
在CTRV中，目標的狀態量爲：

$\vec{x} (t) = (x y v θ ω)^{T}$
其中， $θ$ 爲偏航角，是追蹤的目標車輛在當前車輛座標系下與x軸的夾角，逆時針方向爲正，取值範圍是[ $0$ , $2 π$ ), $ω$ 是偏航角速度。CTRV的狀態轉移函數爲：
$\vec{x} (t + Δ t) = (\begin{matrix} \frac{v}{ω} \sin (ω Δ t + θ) - \frac{v}{ω} \sin (θ) + x (t) \\ - \frac{v}{ω} \cos (ω Δ t + θ) + \frac{v}{ω} \cos (θ) + y (t) \\ v \\ ω Δ t + θ \\ ω \end{matrix})$

本文下面的內容將以CTRV模型作爲我們的運動模型。使用CTRV還存在一個問題，那就是 $ω = 0$ 的情況，此時我們的狀態轉移函數公式中的 $(x, y)$ 將變成無窮大。爲了解決這個問題，我們考察一下 $ω = 0$ 的情況，此時我們追蹤的車輛實際上是直線行駛的，所以我們的 $(x, y)$ 的計算公式就變成了：

x (t + Δ t) = v \cos (θ) Δ t + x (t)

y (t + Δ t) = v \sin (θ) Δ t + y (t)

那麼現在問題來了，我們知道，卡爾曼濾波僅僅用於處理線性問題，那麼很顯然我們現在的處理模型是非線性的，這個時候我們就不能簡單使用卡爾曼濾波進行預測和更新了，此時預測的第一步變成了如下非線性函數：

x_{k + 1} = g (x_{k}, u)

其中，

g ()

表示CTRV運動模型的狀態轉移函數，

u

表示處理噪聲。爲了解決非線性系統下的問題，我們引入擴展卡爾曼濾波（Extended Kalman Filter，EKF）

擴展卡爾曼濾波

雅可比矩陣

擴展卡爾曼濾波使用線性變換來近似非線性線性變換，具體來說，EKF使用一階泰勒展式來進行線性化:

h (x) \approx h (u) + \frac{\partial h (u)}{\partial x} (x - u)

數學中，泰勒公式是一個用函數在某點的信息描述其附近取值的公式。如果函數足夠平滑的話，在已知函數在某一點的各階導數值的情況之下，泰勒公式可以用這些導數值做係數構建一個多項式來近似函數在這一點的鄰域中的值。泰勒公式還給出了這個多項式和實際的函數值之間的偏差。

回到我們的處理模型中，我們的狀態轉移函數爲：

\vec{x} (t + Δ t) = g (x (t)) = (\begin{matrix} \frac{v}{ω} \sin (ω Δ t + θ) - \frac{v}{ω} \sin (θ) + x (t) \\ - \frac{v}{ω} \cos (ω Δ t + θ) + \frac{v}{ω} \cos (θ) + y (t) \\ v \\ ω Δ t + θ \\ ω \end{matrix}), ω \neq 0

\vec{x} (t + Δ t) = g (x (t)) = (\begin{matrix} v \cos (θ) Δ t + x (t) \\ v \sin (θ) Δ t + y (t) \\ v \\ ω Δ t + θ \\ ω \end{matrix}), ω = 0

那麼，對於這個多元函數，我們需要使用多元泰勒級數：

T (x) = f (u) + (x - u) D f (u) + \frac{1}{2!} (x - u)^{2} D^{2} f (u) + . . .

其中，

D f (a)

叫雅可比矩陣，它是多元函數中各個因變量關於各個自變量的一階偏導數構成的矩陣。

在向量微積分中，雅可比矩陣是一階偏導數以一定方式排列成的矩陣，其行列式稱爲雅可比行列式。雅可比矩陣的重要性在於它體現了一個可微方程與給出點的最優線性逼近。因此，雅可比矩陣類似於多元函數的導數。

在擴展卡爾曼濾波中，由於 $(x - u)$ 本身數值很小，那麼 $(x - u)$ 就更小了，所以更高階的級數在此問題中忽略不計，我們只考慮到利用雅可比矩陣進行線性化。

那麼接下來就是求解雅可比矩陣，在CTRV模型中，對各個元素求偏導數可以得到雅可比矩陣（ $ω \neq 0$ ）：

J_{A} = [\begin{matrix} 1 & 0 & \frac{1}{ω} (- \sin (θ) + \sin (Δ t ω + θ)) & \frac{v}{ω} (- \cos (θ) + \cos (Δ t ω + θ)) & \frac{Δ t v}{ω} \cos (Δ t ω + θ) - \frac{v}{ω^{2}} (- \sin (θ) + \sin (Δ t ω + θ)) \\ 0 & 1 & \frac{v}{ω} (- \sin (θ) + \sin (Δ t ω + θ)) & \frac{1}{ω} (\cos (θ) - \cos (Δ t ω + θ)) & \frac{Δ t v}{ω} \sin (Δ t ω + θ) - \frac{v}{ω^{2}} (\cos (θ) - \cos (Δ t ω + θ)) \\ 0 & 0 & 1 & 0 & 0 \\ 0 & 0 & 0 & 1 & Δ t \\ 0 & 0 & 0 & 0 & 1 \end{matrix}]

當 $ω = 0$ 時，雅可比矩陣爲：

J_{A} = [\begin{matrix} 1 & 0 & Δ t \cos (θ) & - Δ t v \sin (θ) & 0 \\ 0 & 1 & Δ t \sin (θ) & Δ t v \cos (θ) & 0 \\ 0 & 0 & 1 & 0 & 0 \\ 0 & 0 & 0 & 1 & Δ t \\ 0 & 0 & 0 & 0 & 1 \end{matrix}]

在我們後面的Python實現中，我們將使用numdifftools包直接計算雅可比矩陣，而不需要我們使用代碼寫這個雅可比矩陣。在得到我們CTRV模型的雅可比矩陣以後，我們的處理模型就可以寫成：

x_{k + 1} = g (x_{k}, u)

P_{k + 1} = J_{A} P_{k} J_{A}^{T} + Q

處理噪聲

處理噪聲模擬了運動模型中的擾動，我們引入運動模型的出發點就是要簡化我們要處理的問題，這個簡化是建立在多個假設的基礎上（在CTRV中，這些假設就是恆定偏航角速度和速度），但是在現實問題中這些假設就會帶來一定的誤差，處理噪聲實際上描述了當我們的系統在運行一個指定時間段以後可能面臨多大的這樣的誤差。在CTRV模型中噪聲的引入主要來源於兩處：直線加速度和偏航角加速度噪聲，我們假定直線加速度和偏航角加速度滿足均值爲 $0$ ，方差分別爲 $σ_{a}^{2}$ , $σ_{\dot{ω}}^{2}$ 的高斯分佈，由於均值爲 $0$ , 我們在狀態轉移公式中的 $u$ 就可以不予考慮，我們來看噪聲帶來的不確定性 $Q$ ，直線加速度和偏航角加速度將影響我們的狀態量 $(x, y, v, θ, ω)$ ，這兩個加速度量對我們的狀態的影響的表達式如下：

n o i s e_t e r m = [\begin{matrix} \frac{1}{2} Δ t^{2} μ_{a} \cos (θ) \\ \frac{1}{2} Δ t^{2} μ_{a} \sin (θ) \\ Δ t μ_{a} \\ \frac{1}{2} Δ t^{2} μ_{\dot{ω}} \\ Δ t μ_{\dot{ω}} \end{matrix}]

其中 $μ_{a}, μ_{\dot{ω}}$ 爲直線上和轉角上的加速度（在這個模型中，我們把把它們看作處理噪聲），我們分解這個矩陣：

n o i s e_t e r m = [\begin{matrix} \frac{1}{2} Δ t^{2} \cos (θ) & 0 \\ \frac{1}{2} Δ t^{2} \sin (θ) & 0 \\ Δ t & 0 \\ 0 & \frac{1}{2} Δ t^{2} \\ 0 & Δ t \end{matrix}] \cdot [\begin{matrix} μ_{a} \\ μ_{\dot{ω}} \end{matrix}] = G \cdot μ

我們知道 $Q$ 就是處理噪聲的協方差矩陣，其表達式爲：

Q = E [n o i s e_t e r m \cdot n o i s e_t e r m^{T}] = E [G μ μ^{T} G^{T}] = G \cdot E [μ μ^{T}] \cdot G^{T}

其中：

E [μ μ^{T}] = (\begin{matrix} σ_{a}^{2} & 0 \\ 0 & σ_{\dot{ω}}^{2} \end{matrix})

所以，我們在CTRV模型中的處理噪聲的協方差矩陣

Q

的計算公式就是：

Q = [\begin{matrix} (\frac{1}{2} Δ t^{2} σ_{a} \cos (θ))^{2} & \frac{1}{4} Δ t^{4} σ_{a}^{2} \sin (θ) \cos (θ) & \frac{1}{2} Δ t^{3} σ_{a}^{2} \cos (θ) & 0 & 0 \\ \frac{1}{4} Δ t^{4} σ_{a}^{2} \sin (θ) \cos (θ) & (\frac{1}{2} Δ t^{2} σ_{a} \sin (θ))^{2} & \frac{1}{2} Δ t^{3} σ_{a}^{2} \sin (θ) & 0 & 0 \\ \frac{1}{2} Δ t^{3} σ_{a}^{2} \cos (θ) & \frac{1}{2} Δ t^{3} σ_{a}^{2} \sin (θ) & Δ t^{2} σ_{a}^{2} & 0 & 0 \\ 0 & 0 & 0 & (\frac{1}{2} Δ t^{2} σ_{\dot{ω}})^{2} & \frac{1}{2} Δ t^{3} σ_{\dot{ω}}^{2} \\ 0 & 0 & 0 & \frac{1}{2} Δ t^{3} σ_{\dot{ω}} & Δ t^{2} σ_{\dot{ω}} \end{matrix}]

測量

假設我們有激光雷達和雷達兩個傳感器，它們分別以一定的頻率來測量如下數據：

激光雷達：測量目標車輛的座標 $(x, y)$ 。這裏的x,y是相對於我們的車輛的座標系的，即我們的車輛爲座標系的原點，我們的車頭爲x軸，車的左側爲y軸。
雷達：測量目標車輛在我們車輛座標系下與本車的距離 $ρ$ ，目標車輛與x軸的夾角 $ψ$ ，以及目標車輛與我們自己的相對距離變化率 $\dot{ρ}$ （本質上就是目標車輛的實際速度在我們和目標車輛的連線上的分量）

前面的卡爾曼濾波器中，我們使用一個測量矩陣 $H$ 將預測的結果映射到測量空間，那是因爲這個映射本身就是線性的，現在，我們使用雷達和激光雷達來測量目標車輛(我們把這個過程稱爲傳感器融合)，這個時候會有兩種情況，即：

激光雷達的測量模型仍然是線性的，其測量矩陣爲：

$H_{L} = [\begin{matrix} 1 & 0 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 & 0 \end{matrix}]$
將預測映射到激光雷達測量空間： $H_{L} \vec{x} = (x, y)^{T}$
雷達的預測映射到測量空間是非線性的，其表達式爲：

$(\begin{matrix} ρ \\ ψ \\ \dot{ρ} \end{matrix}) = (\begin{matrix} \sqrt{x^{2} + y^{2}} \\ a t a n_{2} (y, x) \\ \frac{v x + v y}{\sqrt{x^{2} + y^{2}}} \end{matrix})$
此時我們使用 $h (x)$ 來表示這樣一個非線性映射，那麼在求解卡爾曼增益時我們也要將該非線性過程使用泰勒公式將其線性化，參照預測過程，我們也只要求解 $h (x)$ 的雅可比矩陣：
$J_{H} = [\begin{matrix} \frac{x}{\sqrt{x^{2} + y^{2}}} & \frac{y}{\sqrt{x^{2} + y^{2}}} & 0 & 0 & 0 \\ - \frac{y}{x^{2} + y^{2}} & \frac{x}{x^{2} + y^{2}} & 0 & 0 & 0 \\ \frac{v}{\sqrt{x^{2} + y^{2}}} - \frac{x (v x + v y)}{{(x^{2} + y^{2})}^{\frac{3}{2}}} & \frac{v}{\sqrt{x^{2} + y^{2}}} - \frac{y (v x + v y)}{{(x^{2} + y^{2})}^{\frac{3}{2}}} & \frac{x + y}{\sqrt{x^{2} + y^{2}}} & 0 & 0 \end{matrix}]$

雖然這個雅可比矩陣看似非常複雜，但是我們待會編程的時候並不需要完整的推導出這個雅可比矩陣的表達式，在本篇中，我們採用numdifftools這個公式來直接求解雅可比矩陣。

綜上，EKF的整個過程爲：

Python實現

和之前一樣，爲了實現交互式的代碼，實例的代碼爲了便於理解，我們仍然使用大家熟悉的Python來實現，當然，實際無人車項目肯定是需要用C++來實現的，要將下面的示例代碼使用C++來改寫是非常簡單快速的。

首先引入相關的庫：

from __future__ import print_function
import numpy as np
import matplotlib.dates as mdates
import matplotlib.pyplot as plt
from scipy.stats import norm
from sympy import Symbol, symbols, Matrix, sin, cos, sqrt, atan2
from sympy import init_printing
init_printing(use_latex=True)
import numdifftools as nd
import math

首先我們讀取我們的數據集，該數據集包含了追蹤目標的LIDAR和RADAR的測量值，以及測量的時間點，同時爲了驗證我們追蹤目標的精度，該數據還包含了追蹤目標的真實座標。（數據下載鏈接見文章末尾）

其中第一列表示測量數據來自LIDAR還是RADAR，LIDAR的2，3列表示測量的目標 $(x, y)$ ,第4列表示測量的時間點，第5，6，7，8表示真實的 $(x, y, v_{x}, v_{y})$ , RADAR測量的(前三列)是 $(ρ, ψ, \dot{ρ})$ ，其餘列的數據的意義和LIDAR一樣。

首先我們讀取整個數據：

dataset = []

# read the measurement data, use 0.0 to stand LIDAR data
# and 1.0 stand RADAR data
with open('data_synthetic.txt', 'rb') as f:
    lines = f.readlines()
    for line in lines:
        line = line.strip('\n')
        line = line.strip()
        numbers = line.split()
        result = []
        for i, item in enumerate(numbers):
            item.strip()
            if i == 0:
                if item == 'L':
                    result.append(0.0)
                else:
                    result.append(1.0)
            else:
                result.append(float(item))
        dataset.append(result)
    f.close()

初始化 $P$ ，激光雷達的測量矩陣（線性） $H_{L}$ ，測量噪聲 $R$ , 處理噪聲的中直線加速度項的標準差 $σ_{a}$ ，轉角加速度項的標準差 $σ_{\dot{ω}}$ ：

P = np.diag([1.0, 1.0, 1.0, 1.0, 1.0])
print(P, P.shape)
H_lidar = np.array([[ 1.,  0.,  0.,  0.,  0.],
       [ 0.,  1.,  0.,  0.,  0.]])
print(H_lidar, H_lidar.shape)

R_lidar = np.array([[0.0225, 0.],[0., 0.0225]])
R_radar = np.array([[0.09, 0., 0.],[0., 0.0009, 0.], [0., 0., 0.09]])
print(R_lidar, R_lidar.shape)
print(R_radar, R_radar.shape)

# process noise standard deviation for a
std_noise_a = 2.0
# process noise standard deviation for yaw acceleration
std_noise_yaw_dd = 0.3

在整個預測和測量更新過程中，所有角度量的數值都應該控制在 $[- π, π]$ ，我們知道角度加減 $2 π$ 不變，所以用如下函數表示函數來調整角度：

def control_psi(psi):
    while (psi > np.pi or psi < -np.pi):
        if psi > np.pi:
            psi = psi - 2 * np.pi
        if psi < -np.pi:
            psi = psi + 2 * np.pi
    return psi

使用第一個雷達（或者激光雷達）的測量數據初始化我們的狀態，對於激光雷達數據，可以直接將測量到的目標的 $(x, y)$ 座標作爲初始 $(x, y)$ ，其餘狀態項初始化爲0，對於雷達數據，可以使用如下公式由測量的 $ρ, ψ, \dot{ρ}$ 得到目標的座標 $(x, y)$ ：

x = ρ \times \cos (ψ)

y = ρ \times \sin (ψ)

具體狀態初始化代碼爲：

state = np.zeros(5)
init_measurement = dataset[0]
current_time = 0.0
if init_measurement[0] == 0.0:
    print('Initialize with LIDAR measurement!')
    current_time = init_measurement[3]
    state[0] = init_measurement[1]
    state[1] = init_measurement[2]

else:
    print('Initialize with RADAR measurement!')
    current_time = init_measurement[4]
    init_rho = init_measurement[1]
    init_psi = init_measurement[2]
    init_psi = control_psi(init_psi)
    state[0] = init_rho * np.cos(init_psi)
    state[1] = init_rho * np.sin(init_psi)
print(state, state.shape)

寫一個輔助函數用於保存數值：

# Preallocation for Saving
px = []
py = []
vx = []
vy = []

gpx = []
gpy = []
gvx = []
gvy = []

mx = []
my = []

def savestates(ss, gx, gy, gv1, gv2, m1, m2):
    px.append(ss[0])
    py.append(ss[1])
    vx.append(np.cos(ss[3]) * ss[2])
    vy.append(np.sin(ss[3]) * ss[2])

    gpx.append(gx)
    gpy.append(gy)
    gvx.append(gv1)
    gvy.append(gv2)
    mx.append(m1)
    my.append(m2)

定義狀態轉移函數和測量函數，使用numdifftools庫來計算其對應的雅可比矩陣，這裏我們先設 $Δ t = 0.05$ ，只是爲了佔一個位置，當實際運行EKF時會計算出前後兩次測量的時間差，一次來替換這裏的 $Δ t$ 。

measurement_step = len(dataset)
state = state.reshape([5, 1])
dt = 0.05

I = np.eye(5)

transition_function = lambda y: np.vstack((
    y[0] + (y[2] / y[4]) * (np.sin(y[3] + y[4] * dt) - np.sin(y[3])),
    y[1] + (y[2] / y[4]) * (-np.cos(y[3] + y[4] * dt) + np.cos(y[3])),
    y[2],
    y[3] + y[4] * dt,
    y[4]))

# when omega is 0
transition_function_1 = lambda m: np.vstack((m[0] + m[2] * np.cos(m[3]) * dt,
                                             m[1] + m[2] * np.sin(m[3]) * dt,
                                             m[2],
                                             m[3] + m[4] * dt,
                                             m[4]))

J_A = nd.Jacobian(transition_function)
J_A_1 = nd.Jacobian(transition_function_1)
# print(J_A([1., 2., 3., 4., 5.]))

measurement_function = lambda k: np.vstack((np.sqrt(k[0] * k[0] + k[1] * k[1]),
                                            math.atan2(k[1], k[0]),
                                            (k[0] * k[2] * np.cos(k[3]) + k[1] * k[2] * np.sin(k[3])) / np.sqrt(k[0] * k[0] + k[1] * k[1])))
J_H = nd.Jacobian(measurement_function)
# J_H([1., 2., 3., 4., 5.])

EKF的過程代碼：

for step in range(1, measurement_step):

    # Prediction
    # ========================
    t_measurement = dataset[step]
    if t_measurement[0] == 0.0:
        m_x = t_measurement[1]
        m_y = t_measurement[2]
        z = np.array([[m_x], [m_y]])

        dt = (t_measurement[3] - current_time) / 1000000.0
        current_time = t_measurement[3]

        # true position
        g_x = t_measurement[4]
        g_y = t_measurement[5]
        g_v_x = t_measurement[6]
        g_v_y = t_measurement[7]

    else:
        m_rho = t_measurement[1]
        m_psi = t_measurement[2]
        m_dot_rho = t_measurement[3]
        z = np.array([[m_rho], [m_psi], [m_dot_rho]])

        dt = (t_measurement[4] - current_time) / 1000000.0
        current_time = t_measurement[4]

        # true position
        g_x = t_measurement[5]
        g_y = t_measurement[6]
        g_v_x = t_measurement[7]
        g_v_y = t_measurement[8]

    if np.abs(state[4, 0]) < 0.0001:  # omega is 0, Driving straight
        state = transition_function_1(state.ravel().tolist())
        state[3, 0] = control_psi(state[3, 0])
        JA = J_A_1(state.ravel().tolist())
    else:  # otherwise
        state = transition_function(state.ravel().tolist())
        state[3, 0] = control_psi(state[3, 0])
        JA = J_A(state.ravel().tolist())


    G = np.zeros([5, 2])
    G[0, 0] = 0.5 * dt * dt * np.cos(state[3, 0])
    G[1, 0] = 0.5 * dt * dt * np.sin(state[3, 0])
    G[2, 0] = dt
    G[3, 1] = 0.5 * dt * dt
    G[4, 1] = dt

    Q_v = np.diag([std_noise_a*std_noise_a, std_noise_yaw_dd*std_noise_yaw_dd])
    Q = np.dot(np.dot(G, Q_v), G.T)

    # Project the error covariance ahead
    P = np.dot(np.dot(JA, P), JA.T) + Q

    # Measurement Update (Correction)
    # ===============================
    if t_measurement[0] == 0.0:
        # Lidar
        S = np.dot(np.dot(H_lidar, P), H_lidar.T) + R_lidar
        K = np.dot(np.dot(P, H_lidar.T), np.linalg.inv(S))

        y = z - np.dot(H_lidar, state)
        y[1, 0] = control_psi(y[1, 0])
        state = state + np.dot(K, y)
        state[3, 0] = control_psi(state[3, 0])
        # Update the error covariance
        P = np.dot((I - np.dot(K, H_lidar)), P)

        # Save states for Plotting
        savestates(state.ravel().tolist(), g_x, g_y, g_v_x, g_v_y, m_x, m_y)

    else:
        # Radar
        JH = J_H(state.ravel().tolist())

        S = np.dot(np.dot(JH, P), JH.T) + R_radar
        K = np.dot(np.dot(P, JH.T), np.linalg.inv(S))
        map_pred = measurement_function(state.ravel().tolist())
        if np.abs(map_pred[0, 0]) < 0.0001:
            # if rho is 0
            map_pred[2, 0] = 0

        y = z - map_pred
        y[1, 0] = control_psi(y[1, 0])

        state = state + np.dot(K, y)
        state[3, 0] = control_psi(state[3, 0])
        # Update the error covariance
        P = np.dot((I - np.dot(K, JH)), P)

        savestates(state.ravel().tolist(), g_x, g_y, g_v_x, g_v_y, m_rho * np.cos(m_psi), m_rho * np.sin(m_psi))

這裏有幾點需要注意，首先，要考慮清楚有幾個地方被除數有可能爲 $0$ ，比如說 $ω = 0$ ，以及 $ρ = 0$ 的情況。

處理完以後我們輸出估計的均方誤差（RMSE），並且把各類數據保存以便我們可視化我們的EKF的效果：

def rmse(estimates, actual):
    result = np.sqrt(np.mean((estimates-actual)**2))
    return result

print(rmse(np.array(px), np.array(gpx)),
      rmse(np.array(py), np.array(gpy)),
      rmse(np.array(vx), np.array(gvx)),
      rmse(np.array(vy), np.array(gvy)))

# write to the output file
stack = [px, py, vx, vy, mx, my, gpx, gpy, gvx, gvy]
stack = np.array(stack)
stack = stack.T
np.savetxt('output.csv', stack, '%.6f')

最後我們來看一下我們的EKF在追蹤目標的時候的均方誤差：

0.0736336090893 0.0804598933194 0.229165985264 0.309993887661

我們把我們的EKF的估計結果可視化：

我們放大一個局部看一下：

其中，藍色的點爲我們的EKF對目標位置的估計，橙色的點爲來自LIDAR和RADAR的測量值，綠色的點是目標的真實位置，由此可知，我們的測量是不準確的，因此我們使用EKF在結合運動模型的先驗知識以及融合兩個傳感器的測量的基礎上做出了非常接近目標正是狀態的估計。

EKF的魅力其實還不止在此！我們使用EKF，不僅能夠對目標的狀態（我們關心的）進行準確的估計，從這個例子我們會發現，EKF還能估計我們的傳感器無法測量的量（比如本例中的 $v, ψ, \dot{ψ}$ ）

那麼，擴展卡爾曼濾波就到此結束了，大家可能會問，怎麼沒看到可視化結果那一部分的代碼？哈哈，爲了吸引一波人氣，請大家關注我的博客，我會在下一期更新中（無損卡爾曼濾波）提供這一部分代碼。

最後，細心的同學可能會發現，我們這個EKF執行的效率太低了，實際上，EKF的一個最大的問題就是求解雅可比矩陣計算量比較大，而且相關的偏導數推導也是非常無聊的工作，因此引出了我們下一篇要講的內容，無損卡爾曼濾波（Unscented Kalman Filter，UKF）

無人駕駛汽車系統入門（二）——高級運動模型和擴展卡爾曼濾波

應用於車輛追蹤的高級運動模型

運動模型的狀態轉移公式

擴展卡爾曼濾波

雅可比矩陣

處理噪聲

測量

Python實現

附數據下載鏈接：http://download.csdn.net/download/adamshan/10033533

[轉帖]使用NMT和pmap解決JVM資源泄漏問題原創

Python實現大麥網搶票的四大關鍵技術點解析

Python 安裝庫指令大全

salesforce零基礎學習（一百三十八）零碎知識點小總結（十）

一款開源的.NET程序集反編譯、編輯和調試神器

關於接口協議，你必須要知道這些！

基於 Milvus + LlamaIndex 實現高級 RAG

【2024-05-21】以茶會友

自動駕駛系統進階與項目實戰（五）使用SC-LEGO-LOAM進行較大規模點雲地圖構建和閉環優化

自動駕駛系統進階與項目實戰（四）自動駕駛高精度地圖構建中的三維場景識別和閉環檢測

Move-it無人駕駛黑客馬拉松記錄

自動駕駛系統進階與項目實戰（三）基於全卷積神經網絡的點雲三維目標檢測和ROS實戰

自動駕駛系統進階與項目實戰（二）多激光雷達外參自動化標定算法及代碼實例

https://yachay.unat.edu.pe/blog/index.php?comment_area=format_blog&comment_component=blog&comment_co

linux以太網驅動總結