% Plots demonstrating importance sampling

function impsampplots()

sampsize = 30; % sample size to use for examples

% some data -- a function that's big near 0

xs = -1:.01:1;
ys = foo(xs);
plot(xs, ys, 'LineWidth', 2);
set(gca, 'FontSize', 24);
print -depsc bignearzero.eps
fprintf('true integral %f\n', sum(ys)*2/length(ys))

% integrate by uniform sampling

samp = rand(sampsize,1)*2-1;
plot(xs, ys, 'LineWidth', 2);
hold on;
plot(samp, foo(samp), 'rx', 'MarkerSize', 15);
hold off;
set(gca, 'FontSize', 24);
print -depsc uniformsamp.eps
fprintf('uniform sampling %f\n', 2*sum(foo(samp))/length(samp));

% integrate by importance sampling w/ normal proposal

sig = .25;
samp = randn(sampsize,1)*sig;
wts = exp(-samp.^2/(2*sig^2))/sqrt(2*pi*sig^2);
plot(xs, ys, 'LineWidth', 2);
hold on;
plot(samp, foo(samp), 'rx', 'MarkerSize', 15);
hold off;
set(gca, 'FontSize', 24);
print -depsc importancesamp.eps
fprintf('importance sampling %f\n', sum(foo(samp)./wts)/length(samp));

% plot that demonstrates reason for bias of parallel IS

plot(.3:.1:3, 1./(.3:.1:3), '-', [.5 2], [2 .5], 'x', [1.25 1.25], ...
[.8 1.25], 'o', 'LineWidth', 3, 'MarkerSize', 15);
hold on
plot([.5 2], [2 .5], '--');
hold off
axis([0 3 0 3]);
set(gca, 'FontSize', 24);
xlabel('mean(weights)');
ylabel('1 / mean(weights)');
text(1.3,2.5,'E(mean(weights))', 'FontSize', 20)
line([1.25 1.25], [0 3], 'LineStyle', ':');
%line([.5 .5], [0 2], 'LineStyle', ':');
%line([2 2], [0 .5], 'LineStyle', ':');
line(.5, 0, 'Marker', 'x', 'LineWidth', 3, 'MarkerSize', 15, 'Color', [0 .5 0])
line(2, 0, 'Marker', 'x', 'LineWidth', 3, 'MarkerSize', 15, 'Color', [0 .5 0])
line(1.25, 0, 'Marker', 'o', 'LineWidth', 3, 'MarkerSize', 15, 'Color', [1 0 0]);
print -depsc jensen.eps

% parallel IS example: we observe that we are facing a known
% landmark and 0.9m away from it, but don't know which direction
% we're facing, so our posterior is shaped like part of a ring

n = 500; % sample size
ax = [-2 2 -2 2]; % plot region
xysig = 1; % prior stdev of x, y
thsig = 10; % prior stdev of theta
osig = .3333; % observation stdev
obs = 0.9; % observed distance to landmark
ox = 1; % landmark x
oy = .3; % landmark y

% pick samples from a normal proposal distribution
xs = xysig*randn(n,1);
ys = xysig*randn(n,1);
ths = thsig*randn(n,1);

% plot the proposal samples and the ring around the landmark
d=0:2*pi/100:2*pi;
plot(xs, ys, 'b.', ox, oy, 'rx', ox+obs*cos(d), oy+obs*sin(d), 'r-', ...
'LineWidth', 2);
axis equal;
axis(ax);
print -depsc PISobservation.eps

% compute importance weights
ws = sqrt((xs+obs*cos(ths) - ox).^2 + (ys+obs*sin(ths) - oy).^2);
ws = exp(-ws.^2/(2*osig*osig));

% plot importance weights
maxw = max(ws);
ws = ws./maxw;
plot(ox, oy, 'rx', ox+obs*cos(d), oy+obs*sin(d), 'r-', 'LineWidth', 2);
axis equal;
axis(ax);
for i = 1:n
line(xs(i), ys(i), 'Marker', 'o', 'MarkerSize', 1+10*sqrt(ws(i)))
if (ws(i)>0.5)
line([xs(i), xs(i)+.1*cos(ths(i))], ...
[ys(i), ys(i)+.1*sin(ths(i))]);
end
end
print -depsc PISweights.eps

% normalize theta to -pi, pi
while (sum(ths > pi) > 0)
ths(ths > pi) = ths(ths > pi) - 2*pi;
end
while (sum(ths < -pi) > 0)
ths(ths < -pi) = ths(ths < -pi) + 2*pi;
end

% display posterior mean
fprintf('Expected x y theta pos %f %f %f\n', (ws./sum(ws))' * [xs ys ths]);

% the function that's integrated in the IS examples
function ys = foo(xs)
ys = sin(xs*8).^2.*(xs).^-2;
ys(xs==0) = 64;
return