% Baton twirling example: Monte Carlo sampler for data from a
% marker attached to one end of a spinning stick.

function smcex()

osig = .1; % observation stdev
psig = [.2 .2 .4 .5 1 2]*.4; % proposal stdevs (x y th dx dy dth)
iters = 150; % iterations per time step
plotevery = 15; % how often to plot
ts = 0:.1:2; % time range to consider

% the true answer
[xs, ys, ths, ox, oy] = pos(0, 0, 0, 3, 10, 8, ts);

% corrupt observations with some noise
nox = ox + osig * randn(size(ox));
noy = oy + osig * randn(size(oy));

% plot the observations
plot(nox, noy, 'go');
axis equal;
axis([-.5 6.5 -1 6])
pause

% plot of observations along with snapshots of the baton
plot(ox, oy, 'bo', nox, noy, 'go');
axis equal;
axis([-.5 6.5 -1 6])
for i = 1:length(ts);
line(xs(i)+cos(ths(i))*.6*[1 -1], ys(i)+sin(ths(i))*.6*[1 -1])
end
pause

% starting point for MH
n = 1;
x = randn(n,1);
y = randn(n,1);
th = pi*(2*rand(n,1)-1);
dx = 3+randn(n,1);
dy = 10+2*randn(n,1);
dth = 10*(2*rand(n,1)-1);

% build up #steps
for ct = 3:length(ts);

fprintf('step %d\n', ct);

for i = 1:iters

fprintf(' iter %d', i);

% calc predictions for current point
[xs, ys, ths, ox, oy] = pos(x, y, th, dx, dy, dth, ts(1:ct));

% pick a proposed offset and calc predictions
prop = randn(n, 6) .* repmat(psig * (1-((ct-1)/length(ts))), [n 1]);
px = x + prop(:,1);
py = y + prop(:,2);
pth = th + prop(:,3);
pdx = dx + prop(:,4);
pdy = dy + prop(:,5);
pdth = dth + prop(:,6);
[pxs, pys, pths, pox, poy] = pos(px, py, pth, pdx, pdy, pdth, ts(1:ct));

% compute observation errors and log of likelihood ratios
perr2 = (pox - nox(1:ct)).^2 + (poy - noy(1:ct)).^2;
cerr2 = (ox - nox(1:ct)).^2 + (oy - noy(1:ct)).^2;
lrs = 0.5*((perr2./osig).^2 - (cerr2./osig).^2);

% acceptance test
p = exp(-sum(lrs));
if (rand < p)
x = px;
y = py;
th = pth;
dx = pdx;
dy = pdy;
dth = pdth;
fprintf(' accept\n');
else
fprintf(' reject\n');
end

% plot
if (mod(i, plotevery) == 0)
plot(ox, oy, 'bo', nox, noy, 'go', pox, poy, 'r.');
axis equal;
axis([-1.5 7.5 -2 7])
pause
end

end

end

% Predict the observation sequence by simulating forward from an
% initial state

function [xs, ys, ths, ox, oy] = pos(x, y, th, dx, dy, dth, ts)

g = 9.8; % gravity in m/s^2
r = 0.6; % half-length of baton in m

% simulate rotation and motion of center of gravity
xs = x + dx * ts;
ths = th + dth * ts;
ys = -0.5*g*ts.^2 + dy * ts + y;

% offset to get observations
ox = xs + r*cos(ths);
oy = ys + r*sin(ths);

return;