1.filterpy

FilterPy是一個實(shí)現(xiàn)了各種濾波器的Python模塊，它實(shí)現(xiàn)著名的卡爾曼濾波和粒子濾波器。我們可以直接調(diào)用該庫完成卡爾曼濾波器實(shí)現(xiàn)。其中的主要模塊包括：

• filterpy.kalman

  該模塊主要實(shí)現(xiàn)了各種卡爾曼濾波器，包括常見的線性卡爾曼濾波器，擴(kuò)展卡爾曼濾波器等。

• filterpy.common

  該模塊主要提供支持實(shí)現(xiàn)濾波的各種輔助函數(shù)，其中計算噪聲矩陣的函數(shù)，線性方程離散化的函數(shù)等。

• filterpy.stats

  該模塊提供與濾波相關(guān)的統(tǒng)計函數(shù)，包括多元高斯算法，對數(shù)似然算法，PDF及協(xié)方差等。

• filterpy.monte_carlo

  該模塊提供了馬爾科夫鏈蒙特卡洛算法，主要用于粒子濾波。

開源代碼在：

https://github.com/rlabbe/filterpy/tree/master/filterpy/kalman

我們介紹下卡爾曼濾波器的實(shí)現(xiàn)，主要分為預(yù)測和更新兩個階段，在進(jìn)行濾波之前，需要先進(jìn)行初始化：

• 初始化

預(yù)先設(shè)定狀態(tài)變量dim_x和觀測變量維度dim_z、協(xié)方差矩陣P、運(yùn)動形式和觀測矩陣H等，一般各個協(xié)方差矩陣都會初始化為單位矩陣，根據(jù)特定的場景需要相應(yīng)的設(shè)置。

def __init__(self, dim_x, dim_z, dim_u = 0, x = None, P = None,Q = None, B = None, F = None, H = None, R = None):"""Kalman FilterRefer to http:/github.com/rlabbe/filterpyMethod-----------------------------------------Predict | Update-----------------------------------------| K = PH^T(HPH^T + R)^-1x = Fx + Bu | y = z - HxP = FPF^T + Q | x = x + Ky| P = (1 - KH)P| S = HPH^T + R-----------------------------------------note: In update unit, here is a more numerically stable way: P = (I-KH)P(I-KH)' + KRK'Parameters----------dim_x: intdims of state variables, eg:(x,y,vx,vy)->4dim_z: intdims of observation variables, eg:(x,y)->2dim_u: intdims of control variables,eg: a->1p = p + vt + 0.5at^2v = v + at=>[p;v] = [1,t;0,1][p;v] + [0.5t^2;t]a"""assert dim_x >= 1, 'dim_x must be 1 or greater'assert dim_z >= 1, 'dim_z must be 1 or greater'assert dim_u >= 0, 'dim_u must be 0 or greater'self.dim_x = dim_xself.dim_z = dim_zself.dim_u = dim_u# initialization# predictself.x = np.zeros((dim_x, 1)) if x is None else x # stateself.P = np.eye(dim_x) if P is None else P # uncertainty covarianceself.Q = np.eye(dim_x) if Q is None else Q # process uncertainty for predictionself.B = None if B is None else B # control transition matrixself.F = np.eye(dim_x) if F is None else F # state transition matrix# updateself.H = np.zeros((dim_z, dim_x)) if H is None else H # Measurement function z=Hxself.R = np.eye(dim_z) if R is None else R # observation uncertaintyself._alpha_sq = 1. # fading memory controlself.z = np.array([[None] * self.dim_z]).T # observationself.K = np.zeros((dim_x, dim_z)) # kalman gainself.y = np.zeros((dim_z, 1)) # estimation errorself.S = np.zeros((dim_z, dim_z)) # system uncertainty, S = HPH^T + Rself.SI = np.zeros((dim_z, dim_z)) # inverse system uncertainty, SI = S^-1self.inv = np.linalg.invself._mahalanobis = None # Mahalanobis distance of measurementself.latest_state = 'init' # last process name

• 預(yù)測階段

接下來進(jìn)入預(yù)測環(huán)節(jié)，為了保證通用性，引入了遺忘系數(shù)α，其作用在于調(diào)節(jié)對過往信息的依賴程度，α越大對歷史信息的依賴越小：

?代碼如下：

def predict(self, u = None, B = None, F = None, Q = None):"""Predict next state (prior) using the Kalman filter state propagation equations:x = Fx + BuP = fading_memory*FPF^T + QParameters----------u : ndarrayOptional control vector. If not `None`, it is multiplied by Bto create the control input into the system.B : ndarray of (dim_x, dim_z), or NoneOptional control transition matrix; a value of Nonewill cause the filter to use `self.B`.F : ndarray of (dim_x, dim_x), or NoneOptional state transition matrix; a value of Nonewill cause the filter to use `self.F`.Q : ndarray of (dim_x, dim_x), scalar, or NoneOptional process noise matrix; a value of None will cause thefilter to use `self.Q`."""if B is None:B = self.Bif F is None:F = self.Fif Q is None:Q = self.Qelif np.isscalar(Q):Q = np.eye(self.dim_x) * Q# x = Fx + Buif B is not None and u is not None:self.x = F @ self.x + B @ uelse:self.x = F @ self.x# P = fading_memory*FPF' + Qself.P = self._alpha_sq * (F @ self.P @ F.T) + Qself.latest_state = 'predict'

• 更新階段

按下式進(jìn)行狀態(tài)的更新：

?也可以寫為：

?其中，y是測量余量，S是測量余量的協(xié)方差矩陣。

在實(shí)際應(yīng)用中會做一些微調(diào)，使協(xié)方差矩陣為：

代碼如下：

def update(self, z, R = None, H = None):"""Update Process, add a new measurement (z) to the Kalman filter.K = PH^T(HPH^T + R)^-1y = z - Hxx = x + KyP = (1 - KH)P or P = (I-KH)P(I-KH)' + KRK'If z is None, nothing is computed.Parameters----------z : (dim_z, 1): array_likemeasurement for this update. z can be a scalar if dim_z is 1,otherwise it must be convertible to a column vector.R : ndarray, scalar, or NoneOptionally provide R to override the measurement noise for thisone call, otherwise self.R will be used.H : ndarray, or NoneOptionally provide H to override the measurement function for thisone call, otherwise self.H will be used."""if z is None:self.z = np.array([[None] * self.dim_z]).Tself.y = np.zeros((self.dim_z, 1))returnz = reshape_z(z, self.dim_z, self.x.ndim)if R is None:R = self.Relif np.isscalar(R):R = np.eye(self.dim_z) * Rif H is None:H = self.Hif self.latest_state == 'predict':# common subexpression for speedPHT = self.P @ H.T# S = HPH' + R# project system uncertainty into measurement spaceself.S = H @ PHT + Rself.SI = self.inv(self.S)# K = PH'inv(S)# map system uncertainty into kalman gainself.K = PHT @ self.SI# P = (I-KH)P(I-KH)' + KRK'# This is more numerically stable and works for non-optimal K vs# the equation P = (I-KH)P usually seen in the literature.I_KH = np.eye(self.dim_x) - self.K @ Hself.P = I_KH @ self.P @ I_KH.T + self.K @ R @ self.K.T# y = z - Hx# error (residual) between measurement and predictionself.y = z - H @ self.xself._mahalanobis = math.sqrt(float(self.y.T @ self.SI @ self.y))# x = x + Ky# predict new x with residual scaled by the kalman gainself.x = self.x + self.K @ self.yself.latest_state = 'update'

那接下來，我們就是用filterpy中的卡爾曼濾波器方法完成小車位置的預(yù)測。

2.小車案例

現(xiàn)在利用卡爾曼濾波對小車的運(yùn)動狀態(tài)進(jìn)行預(yù)測。主要流程如下所示：

• 導(dǎo)入相應(yīng)的工具包
• 小車運(yùn)動數(shù)據(jù)生成
• 參數(shù)初始化
• 利用卡爾曼濾波進(jìn)行小車狀態(tài)預(yù)測
• 可視化：觀察參數(shù)的變化與結(jié)果

下面我們看下整個流程實(shí)現(xiàn)：

• 導(dǎo)入包

from matplotlib import pyplot as plt import seaborn as sns import numpy as np from filterpy.kalman import KalmanFilter

• 小車運(yùn)動數(shù)據(jù)生成

在這里我們假設(shè)小車作速度為1的勻速運(yùn)動

# 生成1000個位置，從1到1000，是小車的實(shí)際位置 z = np.linspace(1,1000,1000) # 添加噪聲 mu,sigma = 0,1 noise = np.random.normal(mu,sigma,1000) # 小車位置的觀測值 z_nosie = z+noise

• 參數(shù)初始化

# dim_x 狀態(tài)向量size,在該例中為[p,v]，即位置和速度,size=2 # dim_z 測量向量size，假設(shè)小車為勻速，速度為1，測量向量只觀測位置，size=1 my_filter = KalmanFilter(dim_x=2, dim_z=1)# 定義卡爾曼濾波中所需的參數(shù) # x 初始狀態(tài)為[0,0],即初始位置為0，速度為0. # 這個初始值不是非常重要，在利用觀測值進(jìn)行更新迭代后會接近于真實(shí)值 my_filter.x = np.array([[0.], [0.]])# p 協(xié)方差矩陣，表示狀態(tài)向量內(nèi)位置與速度的相關(guān)性 # 假設(shè)速度與位置沒關(guān)系，協(xié)方差矩陣為[[1,0],[0,1]] my_filter.P = np.array([[1., 0.], [0., 1.]])# F 初始的狀態(tài)轉(zhuǎn)移矩陣，假設(shè)為勻速運(yùn)動模型，可將其設(shè)為如下所示 my_filter.F = np.array([[1., 1.], [0., 1.]])# Q 狀態(tài)轉(zhuǎn)移協(xié)方差矩陣，也就是外界噪聲， # 在該例中假設(shè)小車勻速，外界干擾小，所以我們對F非常確定，覺得F一定不會出錯，所以Q設(shè)的很小 my_filter.Q = np.array([[0.0001, 0.], [0., 0.0001]])# 觀測矩陣 Hx = p # 利用觀測數(shù)據(jù)對預(yù)測進(jìn)行更新，觀測矩陣的左邊一項(xiàng)不能設(shè)置成0 my_filter.H = np.array([[1, 0]]) # R 測量噪聲，方差為1 my_filter.R = 1

• 卡爾曼濾波進(jìn)行預(yù)測

# 保存卡爾曼濾波過程中的位置和速度 z_new_list = [] v_new_list = [] # 對于每一個觀測值，進(jìn)行一次卡爾曼濾波 for k in range(len(z_nosie)):# 預(yù)測過程 my_filter.predict()# 利用觀測值進(jìn)行更新my_filter.update(z_nosie[k])# do something with the outputx = my_filter.x# 收集卡爾曼濾波后的速度和位置信息z_new_list.append(x[0][0])v_new_list.append(x[1][0])

• 可視化

• 預(yù)測誤差的可視化

  # 位移的偏差 dif_list = [] for k in range(len(z)):dif_list.append(z_new_list[k]-z[k]) # 速度的偏差 v_dif_list = [] for k in range(len(z)):v_dif_list.append(v_new_list[k]-1) plt.figure(figsize=(20,9)) plt.subplot(1,2,1) plt.xlim(-50,1050) plt.ylim(-3.0,3.0) plt.scatter(range(len(z)),dif_list,color ='b',label = "位置偏差") plt.scatter(range(len(z)),v_dif_list,color ='y',label = "速度偏差") plt.legend()

  運(yùn)行結(jié)果如下所示：

2.卡爾曼濾波器參數(shù)的變化

首先定義方法將卡爾曼濾波器的參數(shù)堆疊成一個矩陣，右下角補(bǔ)0，我們看一下參數(shù)的變化。

# 定義一個方法將卡爾曼濾波器的參數(shù)堆疊成一個矩陣，右下角補(bǔ)0 def filter_comb(p, f, q, h, r):a = np.hstack((p, f))b = np.array([r, 0])b = np.vstack([h, b])b = np.hstack((q, b))a = np.vstack((a, b))return a

?對參數(shù)變化進(jìn)行可視化：

# 保存卡爾曼濾波過程中的位置和速度 z_new_list = [] v_new_list = [] # 對于每一個觀測值，進(jìn)行一次卡爾曼濾波 for k in range(1):# 預(yù)測過程 my_filter.predict()# 利用觀測值進(jìn)行更新my_filter.update(z_nosie[k])# do something with the outputx = my_filter.xc = filter_comb(my_filter.P,my_filter.F,my_filter.Q,my_filter.H,my_filter.R)plt.figure(figsize=(32,18))sns.set(font_scale=4)#sns.heatmap(c,square=True,annot=True,xticklabels=False,yticklabels==False,cbar=False)sns.heatmap(c,square=True,annot=True,xticklabels=False,yticklabels=False,cbar=False)

對比變換：

從圖中可以看出變化的P，其他的參數(shù)F，Q，H,R為變換。另外狀態(tài)變量x和卡爾曼系數(shù)K也是變化的。

3.概率密度函數(shù)

為了驗(yàn)證卡爾曼濾波的結(jié)果優(yōu)于測量的結(jié)果，繪制預(yù)測結(jié)果誤差和測量誤差的概率密度函數(shù)：

# 生成概率密度圖像 z_noise_list_std = np.std(noise) z_noise_list_avg = np.mean(noise) z_filterd_list_std = np.std(dif_list) import seaborn as sns plt.figure(figsize=(16,9)) ax = sns.kdeplot(noise,shade=True,color="r",label="std=%.3f"%z_noise_list_std) ax = sns.kdeplot(dif_list,shade=True,color="g",label="std=%.3f"%z_filterd_list_std)

結(jié)果如下：

---

總結(jié)：

1.了解filterpy工具包

FilterPy是一個實(shí)現(xiàn)了各種濾波器的Python模塊，它實(shí)現(xiàn)著名的卡爾曼濾波和粒子濾波器。直接調(diào)用該庫完成卡爾曼濾波器實(shí)現(xiàn)。

2.知道卡爾曼濾波的實(shí)現(xiàn)過程

卡爾曼濾波器的實(shí)現(xiàn)，主要分為預(yù)測和更新兩個階段，在進(jìn)行濾波之前，需要先進(jìn)行初始化

• 初始化

預(yù)先設(shè)定狀態(tài)變量和觀測變量維度、協(xié)方差矩陣、運(yùn)動形式和轉(zhuǎn)換矩陣

• 預(yù)測

對狀態(tài)變量X和協(xié)方差P進(jìn)行預(yù)測

• 更新

利用觀測結(jié)果對卡爾曼濾波的結(jié)果進(jìn)行修征

3.能夠利用卡爾曼濾波器完成小車目標(biāo)狀態(tài)的預(yù)測

• 導(dǎo)入相應(yīng)的工具包

• 小車運(yùn)動數(shù)據(jù)生成：勻速運(yùn)動的小車模型

• 參數(shù)初始化：對卡爾曼濾波的參數(shù)進(jìn)行初始化，包括狀態(tài)變量和觀測變量維度、協(xié)方差矩陣、運(yùn)動形式和轉(zhuǎn)換矩陣等

• 利用卡爾曼濾波進(jìn)行小車狀態(tài)預(yù)測：使用Filterpy工具包，調(diào)用predict和update完成小車狀態(tài)的預(yù)測

• 可視化：觀察參數(shù)的變化與結(jié)果

  1.預(yù)測誤差的分布：p,v

  2.參數(shù)的變化：參數(shù)中變化的是X，P，K，不變的是F，Q，H，R

• 誤差的概率密度函數(shù)：卡爾曼預(yù)測的結(jié)果優(yōu)于測量結(jié)果

總結(jié)

以上是生活随笔為你收集整理的智慧交通day02-车流量检测实现05：卡尔曼滤波器实践(小车模型)的全部內(nèi)容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網(wǎng)站內(nèi)容還不錯，歡迎將生活随笔推薦給好友。