function plot_robot_path(P, Q)
% Plot robot initial and final pose over path
% Q = 3D matrix where Q(:,:,i) is pose i
hold on;
[m, ~, k] = size(Q);
px = P(:,1); py = P(:,2); % Extract x- and y-values for convenience

scatter(px, py, 20, 'blue');
axis equal;


for i=1:k
    qx = Q(:,1,i); qy = Q(:,2,i);
    % Plot lines
    for j = 1:m
       plot([px(j) qx(j)], [py(j) qy(j)], 'b:');
    end
    % Plot final shape
    scatter(qx, qy, 20, 'blue');
    
    px = qx;
    py = qy;
end

end

