基本的粒子滤波程序matlab

function ParticleEx1

% Particle filter example adapted from Gordon Salmond and Smith paper.

x = 0.1; % initial state
Q = 1; % process noise covariance
R = 1; % measurement noise covariance
tf = 50; % simulation length

N = 100; % number of particles in the particle filter

xhat = x;
P = 2;
xhatPart = x;

% Initialize the particle filter.
for i = 1 : N
    xpart（i） = x + sqrt（P） * randn;
end

xArr = [x];% 存储状态序列
yArr = [x^2 / 20 + sqrt（R） * randn]; %存储测量序列
xhatArr = [x];% 基于KF的状态序列
PArr = [P];% KF的均方误差序列
xhatPartArr = [xhatPart];% 基于PF的状态序列

close all;

for k = 1 : tf
    % System simulation
    x = 0.5 * x + 25 * x / （1 + x^2） + 8 * cos（1.2*（k-1）） + sqrt（Q） * randn;%状态方程
    y = x^2 / 20 + sqrt（R） * randn;%观测方程
    % Extended Kalman filter
    F = 0.5