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