基于Python實(shí)現(xiàn)粒子濾波效果
更新時(shí)間：2020年12月01日 09:37:01 作者：moge19
這篇文章主要介紹了基于Python實(shí)現(xiàn)粒子濾波效果,本文通過實(shí)例代碼給大家介紹的非常詳細(xì)，對(duì)大家的學(xué)習(xí)或工作具有一定的參考借鑒價(jià)值，需要的朋友可以參考下
1、建立仿真模型
（1）假設(shè)有一輛小車在一平面運(yùn)動(dòng)，起始坐標(biāo)為[0，0]，運(yùn)動(dòng)速度為1m/s，加速度為0.1 m / s 2 m/s^2 m/s2，則可以建立如下的狀態(tài)方程：
Y = A ∗ X + B ∗ U Y=A*X+B*U Y=A∗X+B∗U
U為速度和加速度的的矩陣
U = [ 1 0.1 ] U= \begin{bmatrix} 1 \\ 0.1\\ \end{bmatrix} U=[10.1]
X為當(dāng)前時(shí)刻的坐標(biāo)，速度，加速度
X = [ x y y a w V ] X= \begin{bmatrix} x \\ y \\ yaw \\ V \end{bmatrix} X=⎣⎢⎢⎡xyyawV⎦⎥⎥⎤
Y為下一時(shí)刻的狀態(tài)
則觀察矩陣A為：
A = [ 1 0 0 0 0 1 0 0 0 0 1 0 0 0 0 0 ] A= \begin{bmatrix} 1&0 & 0 &0 \\ 0 & 1 & 0&0 \\ 0 & 0 &1 &0 \\ 0&0 & 0 &0 \end{bmatrix} A=⎣⎢⎢⎡1000010000100000⎦⎥⎥⎤
矩陣B則決定小車的運(yùn)動(dòng)規(guī)矩，這里取B為：
B = [ c o s ( x ) ∗ t 0 s i n ( x ) ∗ t 0 0 t 1 0 ] B= \begin{bmatrix} cos(x)*t &0\\ sin(x)*t &0\\ 0&t\\ 1&0 \end{bmatrix} B=⎣⎢⎢⎡cos(x)∗tsin(x)∗t0100t0⎦⎥⎥⎤
python編程實(shí)現(xiàn)小車的運(yùn)動(dòng)軌跡:
"""
Particle Filter localization sample
author: Atsushi Sakai (@Atsushi_twi)
"""
import math
import matplotlib.pyplot as plt
import numpy as np
from scipy.spatial.transform import Rotation as Rot
DT = 0.1 # time tick [s]
SIM_TIME = 50.0 # simulation time [s]
MAX_RANGE = 20.0 # maximum observation range
# Particle filter parameter
NP = 100 # Number of Particle
NTh = NP / 2.0 # Number of particle for re-sampling
def calc_input():
v = 1.0 # [m/s]
yaw_rate = 0.1 # [rad/s]
u = np.array([[v, yaw_rate]]).T
return u
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 main():
print(__file__ + " start!!")
time = 0.0
# State Vector [x y yaw v]'
x_true = np.zeros((4, 1))
x = []
y = []
while SIM_TIME >= time:
time += DT
u = calc_input()
x_true = motion_model(x_true, u)
x.append(x_true[0])
y.append(x_true[1])
plt.plot(x,y, "-b")
if __name__ == '__main__':
main()
運(yùn)行結(jié)果：
2、生成觀測(cè)數(shù)據(jù)
實(shí)際運(yùn)用中，我們需要對(duì)小車的位置進(jìn)行定位，假設(shè)坐標(biāo)系上有4個(gè)觀測(cè)點(diǎn)，在小車運(yùn)動(dòng)過程中，需要定時(shí)將小車距離這4個(gè)觀測(cè)點(diǎn)的位置距離記錄下來，這樣，當(dāng)小車下一次尋跡時(shí)就有了參考點(diǎn)；
def observation(x_true, xd, u, rf_id):
x_true = motion_model(x_true, u)
# add noise to gps x-y
z = np.zeros((0, 3))
for i in range(len(rf_id[:, 0])):
dx = x_true[0, 0] - rf_id[i, 0]
dy = x_true[1, 0] - rf_id[i, 1]
d = math.hypot(dx, dy)
if d <= MAX_RANGE:
dn = d + np.random.randn() * Q_sim[0, 0] ** 0.5 # add noise
zi = np.array([[dn, rf_id[i, 0], rf_id[i, 1]]])
z = np.vstack((z, zi))
# add noise to input
ud1 = u[0, 0] + np.random.randn() * R_sim[0, 0] ** 0.5
ud2 = u[1, 0] + np.random.randn() * R_sim[1, 1] ** 0.5
ud = np.array([[ud1, ud2]]).T
xd = motion_model(xd, ud)
return x_true, z, xd, ud
3、實(shí)現(xiàn)粒子濾波
#
def gauss_likelihood(x, sigma):
p = 1.0 / math.sqrt(2.0 * math.pi * sigma ** 2) * \
math.exp(-x ** 2 / (2 * sigma ** 2))
return p
def pf_localization(px, pw, z, u):
"""
Localization with Particle filter
"""
for ip in range(NP):
x = np.array([px[:, ip]]).T
w = pw[0, ip]
# 預(yù)測(cè)輸入
ud1 = u[0, 0] + np.random.randn() * R[0, 0] ** 0.5
ud2 = u[1, 0] + np.random.randn() * R[1, 1] ** 0.5
ud = np.array([[ud1, ud2]]).T
x = motion_model(x, ud)
# 計(jì)算權(quán)重
for i in range(len(z[:, 0])):
dx = x[0, 0] - z[i, 1]
dy = x[1, 0] - z[i, 2]
pre_z = math.hypot(dx, dy)
dz = pre_z - z[i, 0]
w = w * gauss_likelihood(dz, math.sqrt(Q[0, 0]))
px[:, ip] = x[:, 0]
pw[0, ip] = w
pw = pw / pw.sum() # 歸一化
x_est = px.dot(pw.T)
p_est = calc_covariance(x_est, px, pw)
#計(jì)算有效粒子數(shù)
N_eff = 1.0 / (pw.dot(pw.T))[0, 0]
#重采樣
if N_eff < NTh:
px, pw = re_sampling(px, pw)
return x_est, p_est, px, pw
def re_sampling(px, pw):
"""
low variance re-sampling
"""
w_cum = np.cumsum(pw)
base = np.arange(0.0, 1.0, 1 / NP)
re_sample_id = base + np.random.uniform(0, 1 / NP)
indexes = []
ind = 0
for ip in range(NP):
while re_sample_id[ip] > w_cum[ind]:
ind += 1
indexes.append(ind)
px = px[:, indexes]
pw = np.zeros((1, NP)) + 1.0 / NP # init weight
return px, pw
4、完整源碼
該代碼來源于https://github.com/AtsushiSakai/PythonRobotics
"""
Particle Filter localization sample
author: Atsushi Sakai (@Atsushi_twi)
"""
import math
import matplotlib.pyplot as plt
import numpy as np
from scipy.spatial.transform import Rotation as Rot
# Estimation parameter of PF
Q = np.diag([0.2]) ** 2 # range error
R = np.diag([2.0, np.deg2rad(40.0)]) ** 2 # input error
# Simulation parameter
Q_sim = np.diag([0.2]) ** 2
R_sim = np.diag([1.0, np.deg2rad(30.0)]) ** 2
DT = 0.1 # time tick [s]
SIM_TIME = 50.0 # simulation time [s]
MAX_RANGE = 20.0 # maximum observation range
# Particle filter parameter
NP = 100 # Number of Particle
NTh = NP / 2.0 # Number of particle for re-sampling
show_animation = True
def calc_input():
v = 1.0 # [m/s]
yaw_rate = 0.1 # [rad/s]
u = np.array([[v, yaw_rate]]).T
return u
def observation(x_true, xd, u, rf_id):
x_true = motion_model(x_true, u)
# add noise to gps x-y
z = np.zeros((0, 3))
for i in range(len(rf_id[:, 0])):
dx = x_true[0, 0] - rf_id[i, 0]
dy = x_true[1, 0] - rf_id[i, 1]
d = math.hypot(dx, dy)
if d <= MAX_RANGE:
dn = d + np.random.randn() * Q_sim[0, 0] ** 0.5 # add noise
zi = np.array([[dn, rf_id[i, 0], rf_id[i, 1]]])
z = np.vstack((z, zi))
# add noise to input
ud1 = u[0, 0] + np.random.randn() * R_sim[0, 0] ** 0.5
ud2 = u[1, 0] + np.random.randn() * R_sim[1, 1] ** 0.5
ud = np.array([[ud1, ud2]]).T
xd = motion_model(xd, ud)
return x_true, 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 gauss_likelihood(x, sigma):
p = 1.0 / math.sqrt(2.0 * math.pi * sigma ** 2) * \
math.exp(-x ** 2 / (2 * sigma ** 2))
return p
def calc_covariance(x_est, px, pw):
"""
calculate covariance matrix
see ipynb doc
"""
cov = np.zeros((3, 3))
n_particle = px.shape[1]
for i in range(n_particle):
dx = (px[:, i:i + 1] - x_est)[0:3]
cov += pw[0, i] * dx @ dx.T
cov *= 1.0 / (1.0 - pw @ pw.T)
return cov
def pf_localization(px, pw, z, u):
"""
Localization with Particle filter
"""
for ip in range(NP):
x = np.array([px[:, ip]]).T
w = pw[0, ip]
# Predict with random input sampling
ud1 = u[0, 0] + np.random.randn() * R[0, 0] ** 0.5
ud2 = u[1, 0] + np.random.randn() * R[1, 1] ** 0.5
ud = np.array([[ud1, ud2]]).T
x = motion_model(x, ud)
# Calc Importance Weight
for i in range(len(z[:, 0])):
dx = x[0, 0] - z[i, 1]
dy = x[1, 0] - z[i, 2]
pre_z = math.hypot(dx, dy)
dz = pre_z - z[i, 0]
w = w * gauss_likelihood(dz, math.sqrt(Q[0, 0]))
px[:, ip] = x[:, 0]
pw[0, ip] = w
pw = pw / pw.sum() # normalize
x_est = px.dot(pw.T)
p_est = calc_covariance(x_est, px, pw)
N_eff = 1.0 / (pw.dot(pw.T))[0, 0] # Effective particle number
if N_eff < NTh:
px, pw = re_sampling(px, pw)
return x_est, p_est, px, pw
def re_sampling(px, pw):
"""
low variance re-sampling
"""
w_cum = np.cumsum(pw)
base = np.arange(0.0, 1.0, 1 / NP)
re_sample_id = base + np.random.uniform(0, 1 / NP)
indexes = []
ind = 0
for ip in range(NP):
while re_sample_id[ip] > w_cum[ind]:
ind += 1
indexes.append(ind)
px = px[:, indexes]
pw = np.zeros((1, NP)) + 1.0 / NP # init weight
return px, pw
def plot_covariance_ellipse(x_est, p_est): # pragma: no cover
p_xy = p_est[0:2, 0:2]
eig_val, eig_vec = np.linalg.eig(p_xy)
if eig_val[0] >= eig_val[1]:
big_ind = 0
small_ind = 1
else:
big_ind = 1
small_ind = 0
t = np.arange(0, 2 * math.pi + 0.1, 0.1)
# eig_val[big_ind] or eiq_val[small_ind] were occasionally negative
# numbers extremely close to 0 (~10^-20), catch these cases and set the
# respective variable to 0
try:
a = math.sqrt(eig_val[big_ind])
except ValueError:
a = 0
try:
b = math.sqrt(eig_val[small_ind])
except ValueError:
b = 0
x = [a * math.cos(it) for it in t]
y = [b * math.sin(it) for it in t]
angle = math.atan2(eig_vec[1, big_ind], eig_vec[0, big_ind])
rot = Rot.from_euler('z', angle).as_matrix()[0:2, 0:2]
fx = rot.dot(np.array([[x, y]]))
px = np.array(fx[0, :] + x_est[0, 0]).flatten()
py = np.array(fx[1, :] + x_est[1, 0]).flatten()
plt.plot(px, py, "--r")
def main():
print(__file__ + " start!!")
time = 0.0
# RF_ID positions [x, y]
rf_id = np.array([[10.0, 0.0],
[10.0, 10.0],
[0.0, 15.0],
[-5.0, 20.0]])
# State Vector [x y yaw v]'
x_est = np.zeros((4, 1))
x_true = np.zeros((4, 1))
px = np.zeros((4, NP)) # Particle store
pw = np.zeros((1, NP)) + 1.0 / NP # Particle weight
x_dr = np.zeros((4, 1)) # Dead reckoning
# history
h_x_est = x_est
h_x_true = x_true
h_x_dr = x_true
while SIM_TIME >= time:
time += DT
u = calc_input()
x_true, z, x_dr, ud = observation(x_true, x_dr, u, rf_id)
x_est, PEst, px, pw = pf_localization(px, pw, z, ud)
# store data history
h_x_est = np.hstack((h_x_est, x_est))
h_x_dr = np.hstack((h_x_dr, x_dr))
h_x_true = np.hstack((h_x_true, x_true))
if show_animation:
plt.cla()
# for stopping simulation with the esc key.
plt.gcf().canvas.mpl_connect(
'key_release_event',
lambda event: [exit(0) if event.key == 'escape' else None])
for i in range(len(z[:, 0])):
plt.plot([x_true[0, 0], z[i, 1]], [x_true[1, 0], z[i, 2]], "-k")
plt.plot(rf_id[:, 0], rf_id[:, 1], "*k")
plt.plot(px[0, :], px[1, :], ".r")
plt.plot(np.array(h_x_true[0, :]).flatten(),
np.array(h_x_true[1, :]).flatten(), "-b")
plt.plot(np.array(h_x_dr[0, :]).flatten(),
np.array(h_x_dr[1, :]).flatten(), "-k")
plt.plot(np.array(h_x_est[0, :]).flatten(),
np.array(h_x_est[1, :]).flatten(), "-r")
plot_covariance_ellipse(x_est, PEst)
plt.axis("equal")
plt.grid(True)
plt.pause(0.001)
if __name__ == '__main__':
main()
到此這篇關(guān)于基于Python實(shí)現(xiàn)粒子濾波的文章就介紹到這了,更多相關(guān)Python實(shí)現(xiàn)粒子濾波內(nèi)容請(qǐng)搜索腳本之家以前的文章或繼續(xù)瀏覽下面的相關(guān)文章希望大家以后多多支持腳本之家！
相關(guān)文章
opencv實(shí)現(xiàn)圖像旋轉(zhuǎn)效果
這篇文章主要為大家詳細(xì)介紹了opencv實(shí)現(xiàn)圖像旋轉(zhuǎn)效果，文中示例代碼介紹的非常詳細(xì)，具有一定的參考價(jià)值，感興趣的小伙伴們可以參考一下2021-03-03
淺談python requests 的put, post 請(qǐng)求參數(shù)的問題
今天小編就為大家分享一篇淺談python requests 的put, post 請(qǐng)求參數(shù)的問題，具有很好的參考價(jià)值，希望對(duì)大家有所幫助。一起跟隨小編過來看看吧2019-01-01
python的random模塊及加權(quán)隨機(jī)算法的python實(shí)現(xiàn)方法
下面小編就為大家?guī)硪黄猵ython的random模塊及加權(quán)隨機(jī)算法的python實(shí)現(xiàn)方法。小編覺得挺不錯(cuò)的，現(xiàn)在就分享給大家，也給大家做個(gè)參考。一起跟隨小編過來看看吧2017-01-01
一篇文章告訴你如何用python進(jìn)行自動(dòng)化測(cè)試,調(diào)用c程序
這篇文章主要介紹了Python實(shí)現(xiàn)性能自動(dòng)化測(cè)試調(diào)用c程序的方法，本文圖文并茂通過實(shí)例代碼相結(jié)合的形式給大家介紹的非常詳細(xì)，具有一定的參考借鑒價(jià)值 ,需要的朋友可以參考下2021-08-08
利用python實(shí)時(shí)刷新基金估值效果(摸魚小工具)
這篇文章主要介紹了利用python實(shí)時(shí)刷新基金估值(摸魚小工具),本文通過實(shí)例代碼給大家介紹的非常詳細(xì)，對(duì)大家的學(xué)習(xí)或工作具有一定的參考借鑒價(jià)值，需要的朋友可以參考下2021-09-09
這篇文章主要介紹了python xml解析實(shí)例詳解的相關(guān)資料,這里舉例說明如何實(shí)現(xiàn)，需要的朋友可以參考下2016-11-11
這篇文章主要介紹了Python3爬樓梯算法,涉及Python基于面向?qū)ο蟮淖址闅v、切片、運(yùn)算等相關(guān)操作技巧,需要的朋友可以參考下2019-03-03
Python使用Turtle模塊繪制國(guó)旗的方法示例
這篇文章主要給大家介紹了關(guān)于Python使用Turtle模塊繪制國(guó)旗的相關(guān)資料，文中通過示例代碼介紹的非常詳細(xì)，對(duì)大家的學(xué)習(xí)或者工作具有一定的參考學(xué)習(xí)價(jià)值，需要的朋友們下面隨著小編來一起學(xué)習(xí)學(xué)習(xí)吧2021-02-02
解決python父線程關(guān)閉后子線程不關(guān)閉問題
這篇文章主要介紹了解決python父線程關(guān)閉后子線程不關(guān)閉問題，具有很好的參考價(jià)值，希望對(duì)大家有所幫助。一起跟隨小編過來看看吧2020-04-04
python獲取文件真實(shí)鏈接的方法,針對(duì)于302返回碼
今天小編就為大家分享一篇python獲取文件真實(shí)鏈接的方法,針對(duì)于302返回碼，具有很好的參考價(jià)值，希望對(duì)大家有所幫助。一起跟隨小編過來看看吧2018-05-05