% 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;