粒子濾波-Particle Filter
歡迎關注《汽車軟體技術》公眾號，回復關鍵字獲取資料。
1. 簡介
粒子濾波是一種濾波演算法，廣泛用於機器人、車輛定位。
基本原理：隨機選取預測域的 N 個點，稱為粒子。以此計算出預測值，並算出在測量域的概率，即權重，加權平均就是最優估計。之後按權重比例，重採樣，進行下次迭代。
此演算法存在的問題是：粒子數量越多，計算越精確，但計算量也會增加，且隨著迭代，有些粒子權重變得很小，導致粒子枯竭。
濾波演算法適用範圍卡爾曼濾波線性高斯分佈粒子濾波非線性，非高斯分佈
更多見：https://blog.csdn.net/zhyongquan/article/details/86611499。
2. 演算法過程
使用matlab描述如下：
X(:,1) = random(i_dist,N,1);
w(:.1) = ones(N,1)/N;
for t =1:T
w(:,t) = pdf(m_dist,y(t)-g(x(:,t));
w(:,t) = w(:,t)/sum(w(:,t));
Resample x(:,t)
x(:,t+1) = f(x(:,t),u(t))+random(t_dist,N,1);
end
2.1 重採樣
v = rand(N,1);
wc = cumsum(w(:,t);
[ ,ind1] = sort([v:wc]);
ind = find(ind1<=N)-(0:N-1);
x(:,t)=x(ind,t);
w(:,t)=ones(N,1)./N;
3. 例子
import numpy as np
import matplotlib.pyplot as plt
def estimate(particles, weights):
"""returns mean and variance of the weighted particles"""
mean = np.average(particles, weights=weights)
var = np.average((particles - mean) ** 2, weights=weights)
return mean, var
def simple_resample(particles, weights):
N = len(particles)
cumulative_sum = np.cumsum(weights)
cumulative_sum[-1] = 1. # 避免攝入誤差
rn = np.random.rand(N)
indexes = np.searchsorted(cumulative_sum, rn)
# 根據索引採樣
particles[:] = particles[indexes]
weights.fill(1.0 / N)
return particles, weights
x = 0.1 # 初始真實狀態
x_N = 1 # 系統過程雜訊的協方差（由於是一維的，這裡就是方差）
x_R = 1 # 測量的協方差
T = 75 # 共進行75次
N = 100 # 粒子數，越大效果越好，計算量也越大
V = 2 # 初始分佈的方差
x_P = x + np.random.randn(N) * np.sqrt(V)
x_P_out = [x_P]
# plt.hist(x_P,N, normed=True)
z_out = [x ** 2 / 20 + np.random.randn(1) * np.sqrt(x_R)] # 實際測量值
x_out = [x] # 測量值的輸出向量
x_est = x # 估計值
x_est_out = [x_est]
# print(x_out)
for t in range(1, T):
x = 0.5 * x + 25 * x / (1 + x ** 2) + 8 * np.cos(1.2 * (t - 1)) + np.random.randn() * np.sqrt(x_N)
z = x ** 2 / 20 + np.random.randn() * np.sqrt(x_R)
# 更新粒子
# 從先驗p(x(k) | x(k - 1))中採樣
x_P_update = 0.5 * x_P + 25 * x_P / (1 + x_P ** 2) + 8 * np.cos(1.2 * (t - 1)) + np.random.randn(N) * np.sqrt(x_N)
z_update = x_P_update ** 2 / 20
# 計算權重
P_w = (1 / np.sqrt(2 * np.pi * x_R)) * np.exp(-(z - z_update) ** 2 / (2 * x_R))
P_w /= np.sum(P_w)
# 估計
x_est, var = estimate(x_P_update, P_w)
# 重採樣
x_P, P_w = simple_resample(x_P_update, P_w)
# 保存數據
x_out.append(x)
z_out.append(z)
x_est_out.append(x_est)
x_P_out.append(x_P)
# 顯示粒子軌跡、真實值、估計值
t = np.arange(0, T)
x_P_out = np.asarray(x_P_out)
for i in range(0, N):
plt.plot(t, x_P_out[:, i], color=gray)
plt.plot(t, x_out, color=lime, linewidth_=2, label=true value)
plt.plot(t, x_est_out, color=red, linewidth_=2, label=estimate value)
plt.legend()
plt.show()
4. 附錄
4.1 中英文對照表
英文中文
Particle Filter例子濾波
Probability Density Function概率密度函數
Resample重採樣
State Space狀態空間
4.2 擴展閱讀
- 《Particle Filter Tutorial 粒子濾波：從推導到應用》（https://blog.csdn.net/heyijia0327/article/details/40899819）
- 《An introduction to particle filters》（https://www.it.uu.se/katalog/andsv164/Teaching/Material/PF_Intro_2014_AndreasSvensson.pdf）
- [Kalman-and-Bayesian-Filters-in-Python（https://github.com/rlabbe/Kalman-and-Bayesian-Filters-in-Python）
推薦閱讀：
