%===============================================================
function hwk6(n)
%===============================================================
global V_ROBOT OMEGA_MAX DT_ODOM BEACON_SD WHEELBASE MOTION_SD;
V_ROBOT=5;          DT_ODOM = 0.1;      OMEGA_MAX=pi/4;     
BEACON_SD = 0.15;   WHEELBASE = 1.0;    MOTION_SD = 0.1;

startX = 15; 
startY = 15;

robot = makeRobot(startX,startY,0,2,'g');
robotHat = makeRobot(startX+10*randn,startY+10*randn,0+pi/8*randn,2,'r');

% Initial Covariance
P = [100 0 0; 0 100 0; 0 0 (pi/18)^2];

for i=1:n
    [x,y] = ginput(1);
    % This is the position of the beacon, the range it is capable of 
    % transmitting and the color it is initially set to.  
    b(i) = makeBeacon(x,y,20,'r');
end

%******************************************
% ADD OTHER CODE HERE
%******************************************

while currentLeg~=9
    % Measurement Update Phase
    for i=1:n
        % Each beacon is tested to see if it can be "heard" by the robot.  
        % Active beacons colors are set to green, and a circle reflecting it
        % measured range to the robot is also plotted.  
        [range,b(i)] = queryBeacon(b(i),robot);        
        if range ~= -1
            [robotHat, P] = MeasurementUpdate(robotHat, P, b(i), range);

%******************************************
% ADD OTHER CODE HERE
%******************************************

        end
    end

    % Time Update Phase
    for j=1:1/DT_ODOM
        [robot, robotHat, P] = TimeUpdate(robot, robotHat, P, legV, legOmega);        
%******************************************
% ADD OTHER CODE HERE
%******************************************
    end

end

%==================================================================================
function [robot, robotHat, P] = TimeUpdate(robot, robotHat, P, v, omega)
%==================================================================================
global DT_ODOM WHEELBASE MOTION_SD;
%******************************************
% ADD OTHER CODE HERE
%******************************************


%==================================================================================
function [robotHat, P] = MeasurementUpdate(robotHat, P, beacon, range)
%==================================================================================
global BEACON_SD;
%******************************************
% ADD OTHER CODE HERE
%******************************************