基于Python实现粒⼦滤波效果
1、建⽴仿真模型
（1）假设有⼀辆⼩车在⼀平⾯运动，起始坐标为[0，0]，运动速度为1m/s，加速度为0.1 m / s 2 m/s^2 m/s2，则可以建⽴如下的状态⽅程：
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为当前时刻的坐标，速度，加速度
X = [ x y y a w V ] X= \begin{bmatrix} x \\ y \\ yaw \\ V \end{bmatrix} X=⎣⎢⎢⎡x yyawV⎦⎥⎥⎤
Y为下⼀时刻的状态
则观察矩阵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则决定⼩车的运动规矩，这⾥取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编程实现⼩车的运动轨迹:
"""
Particle Filter localization sample
author: Atsushi Sakai (@Atsushi_twi)
"""
import math
import matplotlib.pyplot as plt
import numpy as np
from ansform import Rotation as Rot
DT = 0.1 # time tick [s]
toefl是什么SIM_TIME = 50.0 # simulation time [s]
MAX_RANGE = 20.0 # maximum obrvation range
catena# 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 * s(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()
运⾏结果：
2、⽣成观测数据
实际运⽤中，我们需要对⼩车的位置进⾏定位，假设坐标系上有4个观测点，在⼩车运动过程中，需要定时将⼩车距离这4个观测点的位置距离记录下来，这样，当⼩车下⼀次寻迹时就有了参考点；
def obrvation(x_true, xd, u, rf_id):
x_true = motion_model(x_true, u)
# add noi 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 noi
zi = np.array([[dn, rf_id[i, 0], rf_id[i, 1]]])
z = np.vstack((z, zi))
# add noi 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、实现粒⼦滤波
#
def gauss_likelihood(x, sigma):
p = 1.0 / math.sqrt(2.0 * math.pi * sigma ** 2) * \
return p
def pf_localization(px, pw, z, u):
"""
Localization with Particle filter
"""
for ip in range(NP):
knowingx = np.array([px[:, ip]]).T
w = pw[0, ip]
# 预测输⼊
pending
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)
# 计算权重
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)
#计算有效粒⼦数
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)
ba = np.arange(0.0, 1.0, 1 / NP)
re_sample_id = ba + 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、完整源码
"""
Particle Filter localization sample
author: Atsushi Sakai (@Atsushi_twi)
"""
import math
import matplotlib.pyplot as plt
import numpy as np
from ansform 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 obrvation range
# Particle filter parameter
NP = 100 # Number of Particle
NTh = NP / 2.0 # Number of particle for re-sampling河南学位英语>courage什么意思
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 obrvation(x_true, xd, u, rf_id):
x_true = motion_model(x_true, u)
# add noi 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 noi zi = np.array([[dn, rf_id[i, 0], rf_id[i, 1]]])
z = np.vstack((z, zi))
# add noi 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 * s(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) * \
return p
def calc_covariance(x_est, px, pw):
"""
calculate covariance matrix
e 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)
ba = np.arange(0.0, 1.0, 1 / NP)
re_sample_id = ba + np.random.uniform(0, 1 / NP)
indexes = []
ftaapind = 0
for ip in range(NP):
while re_sample_id[ip] > w_cum[ind]:
ind += 1
indexes.append(ind)
px = px[:, indexes]
