SLAM專題（8）卡爾曼濾波和擴展卡爾曼濾波 原理與應用

 

前言：

• 卡爾曼濾波存在着一個非常大的侷限性——它僅能對線性的處理模型和測量模型進行精確的估計，在非線性的場景中並不能達到最優的估計效果。

• 所以之前爲了保證我們的處理模型是線性的，我們上一節中使用了恆定速度模型，然後將估計目標的加減速用處理噪聲來表示，這一模型用來估算行人的狀態其實已經足夠了

• 但是在現實的駕駛環境中，我們不僅要估計行人，我們除了估計行人狀態以外，我們還需要估計其他車輛，自行車等等狀態，他們的狀態估計顯然不能使用簡單的線性系統來描述

• 這裏，我們介紹非線性系統情況下的一種廣泛使用的濾波算法——擴展卡爾曼濾波(Extended Kalman Filter, EKF)

• 本節講解非線性系統中廣泛使用的擴展卡爾曼濾波算法，我們通常將該算法應用於實際的車輛狀態估計（或者說車輛追蹤）中。

• 另外，實際的車輛追蹤運動模型顯然不能使用簡單的恆定速度模型來建模

• 在本節中會介紹幾種應用於車輛追蹤的高級運動模型。

• 並且已經其中的CTRV模型來構造我們的擴展卡爾曼濾波。

• 最後，在代碼實例中，我會介紹如何使用EKF做多傳感器融合。

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

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

1. 一次運動模型（也別稱爲線性運動模型）：

• 恆定速度模型（Constant Velocity, CV）

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

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

2. 二次運動模型：

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

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

3. CTRV

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

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

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

運動模型的狀態轉移公式

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

• 狀態轉移公式：就是我們的處理模型由上一狀態的估計計算下一個狀態的先驗分佈的計算公式，可以理解爲我們基於一定的先驗

• 知識總結出來的運動公式。

 

• 擴展卡爾曼濾波

1. 雅可比矩陣

1. 處理噪聲

 

 

 

• 擴展卡爾曼濾波

1. 雅可比矩陣

1. 處理噪聲

將預測映射到激光雷達測量空間：

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

綜上，EKF的整個過程爲：

4.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)(x,y),第4列表示測量的時間點，第5，6，7，8表示真實的(x,y,vx,vy)(x,y,vx,vy), RADAR測量的(前三列)是(ρ,ψ,ρ˙)(ρ,ψ,ρ˙)，其餘列的數據的意義和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 = 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

 

• 具體狀態初始化代碼爲：

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,ψ,ψ˙）

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

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

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

參考：

• https://blog.csdn.net/AdamShan/article/details/78265754