進行目標跟蹤時，先驗知識告訴我們定位軌跡是平滑的，目標當前時刻的狀態與上一時刻的狀態有關，濾波方法可以將這些先驗知識考慮進來得到更準確的定位軌跡。本文簡單介紹粒子濾波及其使用，接著卡爾曼濾波寫，建議先閱讀室內定位系列（五）——目標跟蹤（卡爾曼濾波）。

原理

這裡跟卡爾曼濾波進行對比來理解粒子濾波。

目標跟蹤中的卡爾曼濾波的簡化版解釋：

定位跟蹤時，可以通過某種定位技術（比如位置指紋法）得到一個位置估計（觀測位置），也可以根據我們的經驗（運動目標常常是勻速運動的）由上一時刻的位置和速度來預測出當前位置（預測位置）。把這個觀測結果和預測結果做一個加權平均作為定位結果，權值的大小取決於觀測位置和預測位置的不確定性程度，在數學上可以證明在預測過程和觀測過程都是線性高斯時，按照卡爾曼的方法做加權是最優的。

上面提到的“線性高斯”需要從概率上來理解。

• 預測過程中，預測的當前位置（或狀態）\(\hat{x}_k^-\)與上一時刻的位置（或狀態）是線性高斯關係，即所謂的運動方程：\[\hat{x}^-_k = A\hat{x}_{k-1} + u_k + \omega_k\]其中的外界輸入\(u_k\)有時可以是零，預測不一定準確，因此存在一個高斯的誤差\(\omega_k\)。從概率分佈上來看，當前位置在預測點附近的概率較大，越遠概越小。
• 觀測過程中，觀測值\(z_k\)與真實狀態\(x_k\)之間也是線性高斯關係，即觀測方程：\[z_k = H_k x_k + n_k\]目標跟蹤中的觀察矩陣\(H\)
  可能就是1，觀察值也並不完全準確，因此也存在一個高斯誤差\(n_k\)。從概率分佈上來看，當前位置在觀測點附近的概率較大，越遠概越小。

預測過程和觀測過程都有一個目標位置的概率分佈，我們應該取一個聯合概率最大的位置作為估計值。如果經驗預測和實際觀測滿足這樣的線性高斯，比如經驗中目標是勻速運動的，那麼直接使用卡爾曼濾波做加權就可以了。

然而，很多情況下並不是線性高斯的，比如以下這種情形：我們除了知道目標常常勻速運動，還知道地圖資訊，如果目標的前方有一堵牆，就不應該繼續用勻速運動（加高斯噪聲）來進行預測，應該有一個完全不一樣的預測值的分佈（比如反向，或者朝牆的同側隨機一個方向，或者沿著牆的邊緣運動），這個分佈取決於我們經驗是怎樣的，它有可能是某個預測值附近的高斯分佈，也可能是某些用公式無法描述的分佈。

既然用公式無法描述，那就用蒙特卡洛方法來模擬：模擬大量的粒子，每個粒子都有一個狀態（位置）和權重，所有這些粒子的狀態分佈和權值共同模擬了目標位置（或狀態）的概率分佈，可以直接把所有粒子的狀態做加權平均得到估計值。在預測過程中，根據經驗給粒子設定一些規則（進行狀態轉移），比如讓每個粒子勻速運動，遇到牆就反向，同時再加上一點隨機性，這樣就能完美地模擬各種經驗和規則了。然後，用觀察結果帶來的概率分佈去更新各個粒子的權重，更加匹配觀測結果的粒子應該賦予更大的權重。

步驟

可以將粒子濾波理解成一個濾波的框架，框架內部應該根據實際問題具體實現。

1. t = 0時，粒子初始化。隨機生成粒子集並設定權值。

2. t = 1, 2, ...， 重複以下步驟：

   a. 預測。根據系統的預測過程預測各個粒子的狀態。

   b. 更新。根據觀測值更新粒子權值。

   c. 重取樣。複製一部分權值高的粒子，同時去掉一部分權值低的粒子。

   d. 輸出：狀態估計。使用粒子和權值估計當前的狀態。

實踐

使用python工具包filterpy來實現卡爾曼濾波和粒子濾波，對knn位置指紋法的定位結果進行濾波。資料來源說明

匯入資料

# 匯入資料
import numpy as np
import scipy.io as scio
offline_data = scio.loadmat('offline_data_random.mat')
online_data = scio.loadmat('online_data.mat')
offline_location, offline_rss = offline_data['offline_location'], offline_data['offline_rss']
trace, rss = online_data['trace'][0:1000, :], online_data['rss'][0:1000, :]
del offline_data
del online_data

# 定位精度
def accuracy(predictions, labels):
    return np.mean(np.sqrt(np.sum((predictions - labels)**2, 1)))

KNN + Kalman Filter

# knn迴歸
from sklearn import neighbors
knn_reg = neighbors.KNeighborsRegressor(40, weights='uniform', metric='euclidean')
knn_reg.fit(offline_rss, offline_location)
knn_predictions = knn_reg.predict(rss)
acc = accuracy(knn_predictions, trace)
print "accuracy: ", acc/100, "m"

accuracy:  2.24421479398 m

# 對knn定位結果進行卡爾曼濾波

from filterpy.kalman import KalmanFilter
from scipy.linalg import block_diag
from filterpy.common import Q_discrete_white_noise
def kalman_tracker():
    tracker = KalmanFilter(dim_x=4, dim_z=2)
    dt = 1.
    # 狀態轉移矩陣
    tracker.F = np.array([[1, dt, 0,  0], 
                          [0,  1, 0,  0],
                          [0,  0, 1, dt],
                          [0,  0, 0,  1]])
    # 用filterpy計算Q矩陣
    q = Q_discrete_white_noise(dim=2, dt=dt, var=0.001)
    # tracker.Q = block_diag(q, q)
    tracker.Q = np.eye(4) * 0.01
    # tracker.B = 0
    # 觀測矩陣
    tracker.H = np.array([[1., 0, 0, 0],
                          [0, 0, 1., 0]])
    # R矩陣
    tracker.R = np.array([[4., 0],
                          [0, 4.]])
    # 初始狀態和初始P
    tracker.x = np.array([[7.4, 0, 3.3, 0]]).T 
    tracker.P = np.zeros([4, 4])
    return tracker

tracker = kalman_tracker()
zs = np.array([np.array([i]).T / 100. for i in knn_predictions]) # 除以100，單位為m
mu, cov, _, _ = tracker.batch_filter(zs) # 這個函式對一串觀測值濾波
knn_kf_predictions = mu[:, [0, 2], :].reshape(1000, 2)
acc = accuracy(knn_kf_predictions, trace / 100.)
print "accuracy: ", acc, "m"

accuracy:  1.76116239607 m

KNN + Particle Filter

# knn迴歸
from sklearn import neighbors
knn_reg = neighbors.KNeighborsRegressor(40, weights='uniform', metric='euclidean')
knn_reg.fit(offline_rss, offline_location)
knn_predictions = knn_reg.predict(rss)
acc = accuracy(knn_predictions, trace)
print "accuracy: ", acc/100, "m"

accuracy:  2.24421479398 m

# 設計粒子濾波中各個步驟的具體實現

from numpy.random import uniform, randn, random, seed
from filterpy.monte_carlo import multinomial_resample
import scipy.stats
seed(7)

def create_particles(x_range, y_range, v_mean, v_std, N):
    """這裡的粒子狀態設定為（座標x，座標y，運動方向，運動速度）"""
    particles = np.empty((N, 4))
    particles[:, 0] = uniform(x_range[0], x_range[1], size=N)
    particles[:, 1] = uniform(y_range[0], y_range[1], size=N)
    particles[:, 2] = uniform(0, 2 * np.pi, size=N)
    particles[:, 3] = v_mean + (randn(N) * v_std)
    return particles

def predict_particles(particles, std_heading, std_v, x_range, y_range):
    """這裡的預測規則設定為：粒子根據各自的速度和方向（加噪聲）進行運動，如果超出邊界則隨機改變方向再次嘗試，"""
    idx = np.array([True] * len(particles))
    particles_last = np.copy(particles)
    for i in range(100): # 最多嘗試100次
        if i == 0:
            particles[idx, 2] = particles_last[idx, 2] + (randn(np.sum(idx)) * std_heading)
        else:
            particles[idx, 2] = uniform(0, 2 * np.pi, size=np.sum(idx)) # 隨機改變方向
        particles[idx, 3] = particles_last[idx, 3] + (randn(np.sum(idx)) * std_v)
        particles[idx, 0] = particles_last[idx, 0] + np.cos(particles[idx, 2] ) * particles[idx, 3]
        particles[idx, 1] = particles_last[idx, 1] + np.sin(particles[idx, 2] ) * particles[idx, 3]
        # 判斷超出邊界的粒子
        idx = ((particles[:, 0] < x_range[0])
                | (particles[:, 0] > x_range[1])
                | (particles[:, 1] < y_range[0]) 
                | (particles[:, 1] > y_range[1]))
        if np.sum(idx) == 0:
            break
            
def update_particles(particles, weights, z, d_std):
    """粒子更新，根據觀測結果中得到的位置pdf資訊來更新權重，這裡簡單地假設是真實位置到觀測位置的距離為高斯分佈"""
    # weights.fill(1.)
    distances = np.linalg.norm(particles[:, 0:2] - z, axis=1)
    weights *= scipy.stats.norm(0, d_std).pdf(distances)
    weights += 1.e-300
    weights /= sum(weights)

def estimate(particles, weights):
    """估計位置"""
    return np.average(particles, weights=weights, axis=0)

def neff(weights):
    """用來判斷當前要不要進行重取樣"""
    return 1. / np.sum(np.square(weights))

def resample_from_index(particles, weights, indexes):
    """根據指定的樣本進行重取樣"""
    particles[:] = particles[indexes]
    weights[:] = weights[indexes]
    weights /= np.sum(weights)
    
def run_pf(particles, weights, z, x_range, y_range):
    """迭代一次粒子濾波，返回狀態估計"""
    x_range, y_range = [0, 20], [0, 15]
    predict_particles(particles, 0.5, 0.01, x_range, y_range) # 1. 預測
    update_particles(particles, weights, z, 4) # 2. 更新
    if neff(weights) < len(particles) / 2: # 3. 重取樣
        indexes = multinomial_resample(weights)
        resample_from_index(particles, weights, indexes)
    return estimate(particles, weights) # 4. 狀態估計

# 對knn定位結果進行粒子濾波

knn_pf_predictions = np.empty(knn_predictions.shape)
x_range, y_range = [0, 20], [0, 15]
n_particles = 50000
particles = create_particles(x_range, y_range, 0.6, 0.01, n_particles) # 初始化粒子
weights = np.ones(n_particles) / n_particles # 初始化權重

for i, pos in enumerate(knn_predictions):
    pos = pos.copy() / 100.
    state = run_pf(particles, weights, pos, x_range, y_range)
    knn_pf_predictions[i, :] = state[0:2]

acc = accuracy(knn_pf_predictions, trace / 100.)
print "final state: ", state
print "accuracy: ", acc, "m"

final state:  [  8.16137026  12.49569879   4.06952385   0.54954716]

accuracy:  1.80881825483 m

軌跡對比

import matplotlib.pyplot as plt
%matplotlib inline
x_i = range(220, 280)
tr, = plt.plot(trace[x_i, 0] / 100., trace[x_i, 1] / 100., 'k-', linewidth=3)
pf, = plt.plot(knn_pf_predictions[x_i, 0], knn_pf_predictions[x_i, 1], 'r-')
kf, = plt.plot(knn_kf_predictions[x_i, 0], knn_kf_predictions[x_i, 1], 'b-')
knn_ = plt.scatter(knn_predictions[x_i, 0] / 100., knn_predictions[x_i, 1] / 100.)
plt.xlabel('x (m)')
plt.ylabel('y (m)')
plt.legend([tr, pf, kf, knn_], ["real trace", "pf", "kf", "knn"])
plt.show()

作者：rubbninja
出處：http://www.cnblogs.com/rubbninja/
關於作者：目前主要研究領域為機器學習與無線定位技術，歡迎討論與指正！
版權宣告：本文版權歸作者和部落格園共有，轉載請註明出處。