粒子滤波matlab代码,粒子滤波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

jArr = [0];

xArr = [x];

yArr = [x^2 / 20 + sqrt(R) * randn];

xhatArr = [x];

PArr = [P];

xhatPartArr = [xhatPart];

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 + 25 * (1 - xhat^2) / (1 + xhat^2)^2;

P = F * P * F' + Q;

H = xhat / 10;

K = P * H' * (H * P * H' + R)^(-1);

xhat = 0.5 * xhat + 25 * xhat / (1 + xhat^2) + 8 * cos(1.2*(k-1));%预测

xhat = xhat + K * (y - xhat^2 / 20);%更新

P = (1 - K * H) * P;

% Particle filter

for i = 1 : N

xpartminus(i) = 0.5 * xpart(i) + 25 * xpart(i) / (1 + xpart(i)^2) + 8 * cos(1.2*(k-1)) + sqrt(Q) * randn;