particle filter粒子滤波

粒子滤波可以用在非高斯分布下的线性和非线性系统。卡尔曼只能用在高斯线性系统，扩展卡尔曼和无迹卡尔曼可以用在高斯非线性系统。适应的场景：KF<-EKF<-UKF<-PF

基本步骤：

1、重要性采样

2、更新权值

3、重采样

4、状态估计

代码实现：

%粒子滤波

N = 200;
Q = 5;
R = 5;
T = 30;
theta = pi / T;
distance = 95 / T;
WorldSize = 100;
X = zeros(2, T);
Z = zeros(2, T);
P = zeros(2, N);
PCenter = zeros(2, T);
w = zeros(N, 1);
err = zeros(1, T);
X(:, 1) = [50 ; 20];
Z(:, 1) = [50 ; 20] + wgn(2, 1, 20 * log10(R));

%初始化粒子集
for i = 1 : N
    P(:, i) = [WorldSize * rand; WorldSize * rand];%初始化粒子集
    dist = norm(P(:, i) - Z(:, i));
    w(i) = (1 / sqrt(R) / sqrt(2 * pi)) * exp(-(dist)^2 / 2 / R);%权重初始化
end


% 粒子滤波过程
for k = 2 : T
    X(:, k) = X(:, k-1) + distance * [(-cos(k * theta)); sin(k * theta)] + wgn(2, 1, 10 * log10(Q));% 状态更新
    Z(:, K) = X(:, k) + wgn(2, 1, 10 * log10(R)); % 观测更新
    for i = 1 : N
        P(:, 1) = P(:, i) + distance * [(-cos(k * theta)); sin(k * theta)] + wgn(2, 1, 10 * log10(R));
        dist = norm(P(:, i) - Z(:, i));
        w(i) = (1 / sqrt(R) / sqrt(2 * pi)) * exp(-(dist) ^ 2 / 2 / R); % 权重更新
    end
    wsum = sum(w);
    for i = 1 : N
        w(i) = w(i) / wsum;% 权重归一化
    end
    % 重采样
    for i = 1 : N
        wmax = 2 * max(w) * rand;
        index = randi(N, 1);
        while(wmax > w(index))
            wmax = wmax - w(index);
            index = index + 1;
            if index > N
                index = 1;
            end
        end
        P(:, i) = p(:, index);
    end
    PCenter(:, k) = sum(P, 2) / N;
    err(k) = norm(X(:, k) - PCenter(:, k));
end