In this lab we will experiment with the velocity and odometry motion models described in Chapter 5 of the textbook. We will try to replicate some of the figures from Chapter 5 using an implementation of the velocity motion model. You will then implement the odometry motion model to replicate some of the figures for the odometry model in the textbook.
Copy and save the following Matlab functions.
vmotion.m: velocity motion model
function p = vmotion(xt, ut, xs, alpha, dt) % VMOTION Velocity motion model % Implements the velocity motion model described in Table 5.1 % of the textbook. All vectors are column vectors, and all angles % are in radians. % % xt - state at time t % ut - control at time t (linear and angular velocities) % xs - state at time t-1 % alpha - noise coefficients % dt - time step duration % % p - probability density % state variables at time t xprime = xt(1); yprime = xt(2); thetaprime = xt(3); % state variables at time t-1 x = xs(1); y = xs(2); theta = xs(3); % control at time t v = ut(1); omega = ut(2); mu = 0.5 * ((x - xprime)*cos(theta) + (y - yprime)*sin(theta)) / ... ((y - yprime)*cos(theta) - (x - xprime)*sin(theta)); xstar = 0.5 * (x + xprime) + mu * (y - yprime); ystar = 0.5 * (y + yprime) + mu * (xprime - x); rstar = sqrt((x - xstar)^2 + (y - ystar)^2); dtheta = atan2(yprime - ystar, xprime - xstar) - ... atan2(y - ystar, x - xstar); vhat = (dtheta / dt) * rstar; omegahat = (dtheta / dt); gammahat = (thetaprime - theta) / dt - omegahat; % noise variances sv = alpha(1) * v^2 + alpha(2) * omega^2; somega = alpha(3) * v^2 + alpha(4) * omega^2; sgamma = alpha(5) * v^2 + alpha(6) * omega^2; p = mvnpdf(v - vhat, 0, sv) * mvnpdf(omega - omegahat, 0, somega) * ... mvnpdf(gammahat, 0, sgamma);
samplevmotion.m: sample from the velocity motion model
function xt = samplevmotion(ut, xs, alpha, dt, n) % SAMPLEVMOTION Sample from the velocity motion model % Implements the motion model sampling algorithm described in % Table 5.3 of the textbook. All vectors are column vectors and % all angles are in radians. % % ut - control at time t (linear and angular velocities) % xs - state at time t-1 % alpha - noise coefficients % dt - time step duration % n - the number of samples to generate % % xt - a 3xn matrix of sampled states (each column representing % a separate sample) xt = zeros(3, n); for a = 1:n % state variables at time t-1 x = xs(1); y = xs(2); theta = xs(3); % control at time t v = ut(1); omega = ut(2); % noise variances sv = alpha(1) * v^2 + alpha(2) * omega^2; somega = alpha(3) * v^2 + alpha(4) * omega^2; sgamma = alpha(5) * v^2 + alpha(6) * omega^2; % generate noisy velocities vhat = v + mvnrnd(0, sv); omegahat = omega + mvnrnd(0, somega); gammahat = 0 + mvnrnd(0, sgamma); % ratio of linear to angular velocities f = vhat / omegahat; xprime = x - f*sin(theta) + f*sin(theta + omegahat*dt); yprime = y + f*cos(theta) - f*cos(theta + omegahat*dt); thetaprime = theta + omegahat*dt + gammahat*dt; xt(:,a) = [xprime; yprime; thetaprime]; end
pglobal.m: computes the motion model over a range of states in a fashion suitable for contour plotting
function [P, X, Y] = pglobal(xt, ut, xs, alpha, dt) % PGLOBAL Evaluate the motion model over a range of states. % [P, X, Y] = pglobal(xt, ut, xs, alpha, dt) computes the approximate % position probability density over a range of states; it does so in % such a way that generating a contour plot of the density is simple. % % You should first generate many samples using SAMPLEVMOTION and then % pass the results to PGLOBAL. % % X and Y are matrices of x and y coordinates suitable for use in the % CONTOUR family of functions. xmin = min(xt(1,:)); xmax = max(xt(1,:)); dx = (xmax - xmin) / 40; ymin = min(xt(2,:)); ymax = max(xt(2,:)); dy = (ymax - ymin) / 40; zmin = min(xt(3,:)); zmax = max(xt(3,:)); dz = (zmax - zmin) / 100; [X,Y,Z] = meshgrid(xmin:dx:xmax, ymin:dy:ymax, zmin:dz:zmax); [nx, ny, nz] = size(X); P = zeros(size(X)); for ix = 1:nx for iy = 1:ny for iz = 1:nz x = X(ix, iy, iz); y = Y(ix, iy, iz); theta = Z(ix, iy, iz); P(ix, iy, iz) = vmotion([x y theta]', ut, xs, alpha, dt); end end end X = X(:,:,1); Y = Y(:,:,1); P = sum(P, 3);
The following script shows you how to replicate Figure 5.3 (a).
% start at origin facing to right xs = [0; 0; 0]; % v = 1 m/s, w = 5 deg/s ut = [1; 5*pi/180]; % noise coefficients alpha = [4 4 4 4 0.1 0.1] * 1e-4; % time step dt = 20; % sample the motion model and plot the results xt = samplevmotion(ut, xs, alpha, dt, 1000); plot(xt(1,:), xt(2,:), '.'); axis equal hold on % now evalate the density of the motion model over the range of xt % and plot the results [P, X, Y] = pglobal(xt, ut, xs, alpha, dt); contour(X, Y, P, 20) hold off
By adjusting the values of alpha you should be able to produce figures similar to Figure 5.3 (b) and (c).
Implement the odometry motion model (Table 5.5) and its accompanying sampling algorithm (Table 5.6) and try to produce figures similar to Figure 5.9.