num_robots = 500;
xbar = [0; 0; pi/2];
xbar_prime = [0.2; 1; pi/4];
X = zeros(3, num_robots);
X_prime = zeros(size(X));
for i = 1:num_robots
    X_prime(:,i) = odometry_model(X(:,i), xbar, xbar_prime);
end
X_prime

%

plot(X_prime(1,:), X_prime(2,:), 'b.')
axis equal

%

function x_prime = odometry_model(x, xbar, xbar_prime)
    d_rot1 = atan2(xbar_prime(2) - xbar(2), xbar_prime(1) - xbar(1)) - xbar(3);
    d_trans = sqrt((xbar_prime(1) - xbar(1))^2 + (xbar_prime(2) - xbar(2))^2);
    d_rot2 = xbar_prime(3) - xbar(3) - d_rot1;
    a = [0 0 0 0];
    d_rot1_hat = d_rot1 + (a(1) * d_rot1^2 + a(2) * d_trans^2) * randn();
    d_trans_hat = d_trans + (a(3) * d_trans^2 + a(4) * d_rot1^2 + a(4) * d_rot2^2) * randn();
    d_rot2_hat = d_rot2 + (a(1) * d_rot2^2 + a(2) * d_trans^2) * randn();
    x_prime = x + [d_trans_hat * cos(x(3) + d_rot1_hat);...
        d_trans_hat * sin(x(3) + d_rot1_hat);...
        d_rot1_hat + d_rot2_hat];
end