% number of times to drive around in a circle
N = 5;

% number of robot states
Nstates = N * 360 + 1;

% true landmark locations
L1 = [10; 0];
L2 = [-10; 0];

% true robot starting position
p0 = [0; 9];

% true robot state and control
theta = 90;
X = zeros(6, Nstates);
X(:, 1) = [-1; 11; L1; L2];
U = zeros(2, Nstates);
for a = 2:Nstates
    p = 9 * [cosd(theta); sind(theta)];
    theta = theta + 1;
    q = 9 * [cosd(theta); sind(theta)];
    X(:, a) = [q; L1; L2];
    U(:, a) = q - p;
end

% true measurements
Z = zeros(4, Nstates);
for a = 2:Nstates
    [~, Z(:, a)] = slam_meas_model(X(:, a));
end

% noisy controls
Unoisy = zeros(2, Nstates);
for a = 2:Nstates
    R = slam_plant_cov(U(:, a));
    Unoisy(:, a) = U(:, a) + mvnrnd(zeros(1, 2), R(1:2, 1:2))';
end


mu = zeros(6, Nstates);
mu(:, 1) = X(:, 1);
Sigma = zeros(6, 6, Nstates);
Sigma(:, :, 1) = eye(6);
for a = 2:Nstates
    [mu(:, a), Sigma(:, :, a)] = kf(mu(:, a - 1), Sigma(:, :, a - 1), Unoisy(:, a), Z(:, a));
end

% plot results for each loop around the circle
for loop = 1:N
    figure
    
    start = (loop - 1) * 360 + 2;
    stop = min([start + 360 Nstates]); 
    idx = start:stop;
    
    % true path
    plot(X(1, idx), X(2, idx), 'k');
    hold on
    
    % KF estimated path
    plot(mu(1, idx), mu(2, idx), 'b')
    
    % covariances every 20ish degrees
    for a = start:20:stop
        sigma = Sigma(:, :, a);
        
        % for this problem sigma(1, 1) should equal sigma(2, 2)
        radius = sqrt(sigma(1, 1));
        circle(mu(1:2, a), radius, 'k');
    end
    axis equal
    s = sprintf('Loop %d', loop);
    title(s);
    xlabel('x coord');
    ylabel('y coord');
end

