粒子滤波器

粒子滤波器是一种常见的非线性滤波器，与卡尔曼类滤波器相比，它的适用性更广。
粒子滤波器的主要步骤有：
1、粒子集初始化
2、更新粒子集权重
3、重采样
4、计算估计值和协方差矩阵

1、粒子集初始化
根据初始状态$X_0$的先验概率矩阵$p(X_0)$生成$N$个粒子${\chi }_0^{(i)}$，每个粒子权重初始化为$w_0({\chi }_0^{(i)})=\frac{1}{N}$。
2、更新粒子集权重
$$w_n({\chi }_n^{(i)})=w_{n-1}({\chi }_{n-1}^{(i)})\cdot {\alpha }_n({\chi }_{n-1}^{(i)},Z_{n-1}^{(i)})$$
这里的${\alpha }_n({\chi }_{n-1}^{(i)},Z_{n-1}^{(i)})$可以是将观测误差映射到高斯分布得到的系数，$w_n({\chi }_n^{(i)})$归一化后即为新的权重。
3、重采样
在经过多次递推后，有些粒子的权重趋向1，有些则趋向0，这种现象称为粒子退化。为了避免粒子退化，可以引入重采样。

• 首先，生成[0,1]之间均匀分布的随机数组$r$，计算权重的累加数组${\sum }_{i=0}^N{w}_n({\chi }_n^{(i)})$
• 当$r_i>{\sum }_{i=0}^{j-1}{w}_n({\chi }_n^{(i)})$且$r_i<{\sum }_{i=0}^j{w}_n({\chi }_n^{(i)})$时，将${\chi }_j$加入重采样粒子集，成为第$i$个重采样粒子，权重更新为$\frac{1}{N}$

4、计算估计值和协方差矩阵
这里就是按照求均值和方差的公式求解。
实现：

import math
import matplotlib.pyplot as plt
import numpy as np
from matplotlib.animation import FuncAnimation


#  Simulation parameter
Q_sim = np.diag([0.4]) ** 2
R_sim = np.diag([0.8, np.deg2rad(10.0)]) ** 2

DT = 0.1  # time tick [s]
SIM_TIME = 65.0  # simulation time [s]
MAX_RANGE = 200.0  # maximum observation range


# Particle filter parameter
NP = 100  # Number of Particle
NTh = NP / 2.0  # Number of particle for re-sampling
DT = 0.5  # time tick [s]
# Estimation parameter of PF
R = np.diag([0.2]) ** 2  # range error
Q = np.diag([2.0, np.deg2rad(40.0)]) ** 2  # input error


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(xEst, px, pw):
    cov = np.zeros((3, 3))

    for i in range(px.shape[1]):
        dx = (px[:, i] - xEst)[0:3]
        cov += pw[0, i] * dx.dot(dx.T)
    cov /= NP

    return cov



def re_sampling(px, pw):
    """
    low variance re-sampling
    """

    w_cum = np.cumsum(pw)
    base = np.arange(0.0, 1.0, 1.0/NP)
    re_sample_id = base + np.random.uniform(0, 1.0/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 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() * Q[0, 0] ** 0.5
        ud2 = u[1, 0] + np.random.randn() * Q[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(R[0, 0]))

        px[:, ip] = x[:, 0]
        pw[0, ip] = w

    pw = pw / pw.sum()  # normalize

    xEst = px.dot(pw.T)
    PEst = calc_covariance(xEst, 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 xEst, PEst, px, pw


#constant velocity inuput
def calc_input():
    v = 1  # [m/s]
    yaw_rate = 0.1  # [rad/s]
    u = np.array([[v, yaw_rate]]).T
    return u




def observation(xTrue, xd, u, RF_ID):
    xTrue = motion_model(xTrue, u)

    # add noise to gps x-y
    z = np.zeros((0, 3))

    for i in range(len(RF_ID[:, 0])):

        dx = xTrue[0, 0] - RF_ID[i, 0]
        dy = xTrue[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 xTrue, z, xd, ud


def main():
    print(__file__ + " start!!")


    time = 0.0

    # RF_ID positions [x, y]
    RFi_ID = np.array([[15.0,  0.0],
                       [15.0,  20.0],
                       [-15.0, 0.0],
                       [-15.0, 20.0]])

    # State Vector [x y yaw v]'
    xEst_1 = np.zeros((4, 1))

    xTrue = np.zeros((4, 1))
    PEst_1 = np.eye(4)
    # Particle filter parameter

    px = np.zeros((4, NP))  # Particle store
    pw = np.zeros((1, NP)) + 1.0 / NP  # Particle weight
    xDR = np.zeros((4, 1))  # Dead reckoning

    # history
    hxEst_1 = np.zeros((4, 1))
    hxTrue = xTrue
    hxDR = np.zeros((4, 1))
    hxcount = np.zeros((1, 1))
    temp = np.zeros((1, 1))

    while SIM_TIME >= time:
        time += DT
        u = calc_input()

        xTrue, z, xDR, ud = observation(xTrue, xDR, u, RFi_ID)

        xEst_1, PEst_1, px, pw = pf_localization(px, pw, z, ud) # particle filter
        
        # store data history
        hxEst_1 = np.hstack((hxEst_1, xEst_1))
        hxTrue = np.hstack((hxTrue, xTrue))
    fig, ax = plt.subplots()
    xdata, ydata = [], []
    ln, = ax.plot([], [], 'r-', animated=False)
    l = ax.plot(hxTrue.T[:,0], hxTrue.T[:,1])

    def init():
        ax.set_xlim(-25, 25)
        ax.set_ylim(-10, 30)
        return l,

    def update(frame):
        xdata.append(frame[0])
        ydata.append(frame[1])
        ln.set_data(xdata, ydata)
        return ln,

    ani = FuncAnimation(fig, update, frames=hxEst_1[:2,:].T,
                        init_func=init)
    ani.save('particle_filter.gif', writer='imagemagick', fps=10)

    plt.show()
if __name__ == '__main__':
    main()

结果：

参考：
https://blog.csdn.net/piaoxuezhong/article/details/78619150
https://www.stats.ox.ac.uk/~doucet/doucet_johansen_tutorialPF2011.pdf
https://m.douban.com/book/subject/1250131