%%% Section 1.5 
%%% State-Space Measurement Modeling

%x = Ax + Bu 
%Y = Cx + Du

clear
close all

X = [0;1];  %initial state, pos = 0, vel = 1m/s
dt = 0.1;   %time step 0.1 seconds
T = 1;     %duration of simulation, 0 to T seconds


A = [1 dt; 0 1];
B = [dt.^2/2;dt];
m = 2;
u = 0;

C = [ones(4,1),zeros(4,1)];  %direct measurement of state;
sigma = 0.08;   %standard deveation in meters

for i=1:(T/dt)
    X(:,i+1) = A*X(:,i) + B*u./m;
    Y(:,i+1) = C*X(:,i+1) + sigma*randn(4,1);
end

figure(1)
plot([0:dt:T],X)
%legend('position','velocity')
xlabel('time [sec]')
ylabel('meters, m/s')
hold on
for n=1:4
    scatter([0:dt:T],Y(n,:))
end
plot([0:dt:T],mean(Y),'+-')
legend('position','velocity','z1','z2','z3','z4','LLSE')
%ylim([-.1,1.1])

%%% 1000x

for n1 = 1:100
    
    X = [0;1];  %initial state, pos = 0, vel = 1m/s
    dt = 0.1;   %time step 0.1 seconds
    T = 1;     %duration of simulation, 0 to T seconds
    
    
    A = [1 dt; 0 1];
    B = [dt.^2/2;dt];
    m = 2;
    u = 0;
    
    C = [ones(4,1),zeros(4,1)];  %direct measurement of state;
    sigma = 0.2;   %standard deveation in meters
    
    Y(:,1) = C*X(:,1) + sigma*randn(4,1);
    for i=1:(T/dt)
        X(:,i+1) = A*X(:,i) + B*u./m;
        Y(:,i+1) = C*X(:,i+1) + sigma*randn(4,1);
    end
    
    Y_llse(:,n1) = mean(Y)';   
end


figure(2)
plot([0:dt:T],X(1,:))
%legend('position','velocity')
xlabel('time [sec]')
ylabel('meters')
hold on
errorbar(X(1,:),mean(Y_llse'),std(Y_llse'),'-s','MarkerSize',10,...
     'MarkerEdgeColor','red','MarkerFaceColor','red')
for n=1:100
    scatter([0:dt:T],Y_llse(:,n),'b','MarkerEdgeAlpha',.1)
end 
legend('position','1 std','LLSE')
errorbar(X(1,:),mean(Y_llse'),std(Y_llse'),'-s','MarkerSize',10,...
     'MarkerEdgeColor','red','MarkerFaceColor','red')

for n=1:T/dt+1
    %text(X(1,n)-dt/2,mean(Y_llse(n,:)')+.3,['sigma = ',num2str(std(Y_llse(n,:)'))])
end
n = 5
text(X(1,n)-3*dt,mean(Y_llse(n,:)')+.3,['sigma = ',num2str(std(Y_llse(n,:)'))])