粒子滤波方法入门

function [ output_args] = Untitled( input_args )
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 模型：在一维空间，用非线性状态方程和观测方程，用例子滤波方法进行数模分析
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% 清零
close all;
clear all;
clc;

%% 第1段：噪声变化与噪声模拟
tf = 100;       %时间模拟长度
w = rand(1,tf); %rand函数产生由在(0, 1)之间均匀分布的随机数组成的数组，本文为一个1行100列的数组
Q = var(w);     %求方差公式。
W = sqrt(Q)*w;  %过程噪声，标准差乘以随机数。

vnoise = randn(1,tf);%randn：产生均值为0，方差σ^2 = 1，标准差σ = 1的正态分布的随机数或矩阵的函数
R = var(vnoise);     %求方差
V = sqrt(R)*vnoise;  %观测噪声

%% 第2段：状态方程于观测方程、滤波估计机器初始值设定
x = 0.1;        %真实状态初始值
x_estimate = 1; %真实状态初始值的估计值
p_x_estimate = x_estimate;%PF的初始估计值（没有粒子滤波之前的初始估计值，当例子滤波开始后，就会逐步与真实状态值靠近）
pf_P = 5;       %PF误差方差初始值
x_array = [x];  %真实值数组（真实值观测不到的，但是是实际的状态）
p_x_estimate_array = [p_x_estimate];%PF最优估计值数组,把初始估计值加进来作为第一个。
N = 500;        %粒子滤波的粒子数

%% 第3段：非线性数字模拟系统，计算得到PK估计值
% 第一步：初始化状态，利用状态初始值x_estimate = 1，用大量例子模拟X(t),粒子在空间均匀分布；生成例子滤波初始N个例子

for i = 1:N         %N = 500
    p_xpart(i) = p_x_estimate + sqrt(pf_P)*rand;    %最好均匀分布randn
end

for k = 1:tf    %在100个时间步长里计算
    %模拟非线性系统
    x = 0.5 * x + (25 * x / (1 + x^2)) + 8 * cos(1.2*(k-1)) + W(k);%状态值（实际值）.其中是W是个1*100的数组。W(k)是第k个。
    y = (x^2/20) + V(k); %观测值（实际值的观测值）
    % 第二步：预测阶段，根据状态转移方程，每一个粒子得到一个预测粒子
    for i = 1 : N        %N = 500，粒子也要代入状态方程。这一步就是实际值和粒子根据状态方程得出的结果之差，来衡量每个粒子的重要程度。
        p_xpartminus(i) = 0.5 * p_xpart(i) + (25 * p_xpart(i)) / (1 + p_xpart(i)^2) + 8 * cos(1.2*(k-1)) + W(k); 
        %第三步：校正阶段，将生成的粒子带入含噪的状态方程，对预测粒子进行评价，越接近于真实状态的粒子，其权重越大；
        p_ypart = p_xpartminus(i)^2 / 20; %利用观测方程计算观测预测值，在此计算时，一般不加观测方程里的误差。
        p_vhat = y - p_ypart;             %含噪观测值和粒子观测预测值的差
        p_q(i) = (1 / sqrt(R) / sqrt(2*pi)) * exp(-p_vhat^2 / 2 / R); %根据差值给出100个粒子对应的权重,差越小权重越大
    end
    % 平均每一个估计的可能性
    p_qsum = sum(p_q);
    for i = 1:N
        p_q(i) = p_q(i) / p_qsum; %各个粒子进行权值归一化
    end
    
    % 第四步：重采样，根据粒子权重对例子进行筛选，筛选过程中，既要大量保留权重大的粒子，又要有一小部分权重小的粒子
    for i=1:N %在这里，依次赋值给p_xpart(1)、p_xpart(2)。。。
        p_u = rand; %均匀分布
        p_qtempsum = 0;
    	for j = 1:N %先产生一个均匀分布的随机数，比如0.5，然后让p_q(j)累计相加，当p_qtempsum >= p_u的那一瞬间，其对应的值赋给p_xpart(1).
            p_qtempsum = p_qtempsum + p_q(j);
        	if p_qtempsum >= p_u
                p_xpart(i) = p_xpartminus(j);%在这里 p_xpart(i)实现循环赋值，上边比较的是权值累加，这里赋值的是粒子值。
                break;
            end
        end
    end
    %第五步：滤波，将重采样后的粒子带入状态转移方程得到新的预测粒子，即步骤2，
    p_x_estimate = mean(p_xpart);%求重采样后的粒子值的平均值，就是最终的预测值。且粒子值p_xpart会代入状态方程进入下一个循环。
    
    %进行画图程序
    x_array = [x_array,x];%真实值数组
    p_x_estimate_array = [p_x_estimate_array,p_x_estimate];%预测值数组。
    p_error(k,:) = abs(x_array(k) - p_x_estimate_array(k));%误差数组。
    
end

%% 第4段：粒子滤波绘图
t = 0:tf;
figure(1);
plot(t, x_array, '-r', t, p_x_estimate_array, 'g-o');
xlabel('时间步长');
ylabel('状态');
legend('含噪状态值','PF估计值');
title('含噪状态信号和粒子滤波估计信号');

figure(2);
plot(t, x_array, '-r', t, p_x_estimate_array, 'g-o', t, p_x_estimate_array-1.96*sqrt(pf_P), 'b:', t, p_x_estimate_array+1.96*sqrt(pf_P), 'b')
xlabel('时间步长');
ylabel('状态');
legend('含噪状态值','PF估计值','95%置信区间');
title('含噪状态信号和粒子滤波估计信号及其置信区间');

%% 第5段：误差分析及其绘图
p_xrms = sqrt((norm(x_array - p_x_estimate_array)^2)/tf);
disp(['PF估计误差均方值=', num2str(p_xrms)]);
t = 1:tf;
figure(3);
plot(t,p_error);
xlabel('时间步长');
ylabel('状态');
grid
title('粒子滤波估信号与状态真实信号误差')

end