hybrid_mc.m

粒子滤波的源代码我自己觉得特别好可以了解粒子滤波有一个基础

% Hybrid MC demo          niter=10;

function hybrid_mc(niter);

C=[1 0.98; 0.98 1];
Ci=inv(C);
m=[0 0];

clf;

x=[-1 1];
px=exp(-(x-m)*Ci*(x-m)'/2);

plot_gaussian(sqrt(2)*C,m,2,60);
axis([-3 3 -3 3]);
set(gcf,'Renderer','zbuffer');
pause;

for i=1:niter,
xold=x;
pxold=exp(-(xold-m)*Ci*(xold-m)'/2);

% sample momentum
p=randn(1,2);

% follow dynamics
[x, p]=leapfrog(x,p,20,0.05,Ci,m);

% compute new energy
px=exp(-(x-m)*Ci*(x-m)'/2);

% accept or reject
if rand<min(1,px/pxold), % accept
plot(xold(1),xold(2),'wo');
plot(x(1),x(2),'b.');
hold on;
plot(x(1),x(2),'bo');
fprintf('a');
else
plot(x(1),x(2),'r.');
fprintf('r');
x=xold;
end;

drawnow;
pause(0.9);
end;


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [x, p]=leapfrog(x,p,n,eps,Ci,m)

% global Ci;
% global m;

% first half step
p2=p-(eps/2)*(x-m)*Ci;
x=x+eps*p2;

% n-1 full steps
for i=1:n-1;
xo=x;
x=x+eps*p-(eps*eps/2)*(x-m)*Ci;
p=p-(eps/2)*((x+xo-2*m)*Ci);
plot(xo(1),xo(2),'w.');
plot(x(1),x(2),'k.');
pause(0.03);
drawnow;
end;
% last half step