clear all;close all;clc;
%% Batch LS Estimator.
time = [0 1 2 3 4 10 12 18]'; H = time;
Dist = [4.71 9 15 19 20 45 55 78]'; z = Dist;
invR = diag(0.9.^(7:-1:0));
% The estimate of the speed.
v_batch = (H'*invR*H)^-1*H'*invR*z; 
%% Recursive LS Estimator
P = 1e6; % Initial covariance.
v_recursive = 0; % First estimate.
v_recursive_plot = zeros(1,8);
P_vector = zeros(1,8);
% LS estimator algorithm
for i = 1:8
    S = H(i,:)*P*H(i,:)' + inv(invR(i,i)); 
    W = P*H(i,:)'*S^-1;
    P = P - W*S*W';
    P_vector(i) = P;
    v_recursive = v_recursive + W*(z(i)-H(i,:)*v_recursive);
    % Recursive speed estimation each recursion
    v_recursive_plot(i) = v_recursive;
end

v_batch_plot = ones(1,8) * v_batch;
figure(1);
subplot(211);
plot(time, v_batch_plot, 'b--', 'LineWidth', 2, 'DisplayName', 'Batch LS Estimate');hold on;
axis tight; grid on;
plot(time, v_recursive_plot, 'r-.', 'LineWidth', 2, 'DisplayName', 'Recursive LS Estimates'); hold off;
legend show;
xlabel('Time');ylabel('Speed');
title('Speed profile of the vehicle.');
% print -dpng fig.png
subplot(212);
plot(time, P_vector, 'b--', 'LineWidth', 2, 'DisplayName', 'Batch LS Estimate');hold on;
axis tight; grid on;
xlabel('Time');ylabel('P(k)');
title('Propagation of the covariance matrix.');
print -dpng fig.png