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

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

clear
close all

X = [1;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

Cd = [1 -dt; 0 1];


Q = 10;     %Newtons of Force std

x_est = [0;0];
Y(:,1) = C*X(:,1) + sigma*randn(4,1);
    
%x_est(1) = mean(Y);

R = 9999*eye(2);

u = zeros(1,T/dt+1);
uN = Q*randn(1,10);

for i=1:(T/dt)
    X(:,i+1) = A*X(:,i) + B*(u(:,i) + uN(:,i))/m ; 
    %x_est(:,i+1) = A*x_est(:,i) + B*u(:,i)/m;
%     ps = sqrt(ps^2 + dt*vs^2 + dt^2/2*sigma^2);
%     vs = sqrt(vs^2 + dt*sigma^2);
    %R = A*R*A' + B*Q^2*B';
    
    
    Y(:,i+1) = C*X(:,i+1) + sigma*randn(4,1);
    
    Yd = x_est(:,i) + [-dt^2/2;dt]*u(1,i);
    Wd = [sqrt(R(1,1) + dt^4/4*Q^2), sqrt(R(2,2) + dt^2*Q^2)];   %var(x1) + dt^4/4*var(u)
    
    Cf = [C;Cd]; 
    Yf = [Y(:,i+1);Yd];
    Wf = diag(1./[sigma sigma sigma sigma Wd].^2);
    
    x_est(:,i+1) = inv(Cf'*Wf*Cf)*Cf'*Wf*Yf;
    R = inv(Cf'*Wf*Cf);
    x_est(:,i+1);
%     Wd
%     sqrt(R)    
end

x_est


figure(1)
plot([0:dt:T],X(1,:),'k')
%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],x_est(1,:)','r+-')
legend('position','z1','z2','z3','z4','LLSE pos')
%legend('position','velocity','z1','z2','z3','z4','LLSE pos','LLSE vel')
%ylim([-.1,1.1])



%%%%% repeat 100x

%uN = Q*randn(1,T/dt);
for n1 = 1:1000

    X = [1;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
    
    Cd = [1 -dt; 0 1];
    
    
    Q = 1;     %Newtons of Force std
    
    x_est = [0;0];
    Y = C*X(:,1) + sigma*randn(4,1);
    
    %x_est(1) = mean(Y);
    
    R = 99999*eye(2);
    
    u = zeros(1,T/dt+1);
    %uN = 0*Q*randn(1,T/dt);     %(Must be same path each iteration)
    
    
    for i=1:(T/dt)
        X(:,i+1) = A*X(:,i) + B*(u(:,i) + uN(:,i))/m ;
        %x_est(:,i+1) = A*x_est(:,i) + B*u(:,i)/m;
        %     ps = sqrt(ps^2 + dt*vs^2 + dt^2/2*sigma^2);
        %     vs = sqrt(vs^2 + dt*sigma^2);
        %R = A*R*A' + B*Q^2*B';
        
        
        Y(:,i+1) = C*X(:,i+1) + sigma*randn(4,1);
        
        Yd = x_est(:,i) + [-dt^2/2;dt]*u(1,i);
        Wd = [sqrt(R(1,1) + dt^4/4*Q^2), sqrt(R(2,2) + dt^2*Q^2)];   %var(x1) + dt^4/4*var(u)
        
        Cf = [C;Cd];
        Yf = [Y(:,i+1);Yd];
        Wf = diag(1./[sigma sigma sigma sigma Wd].^2);
        
        x_est(:,i+1) = inv(Cf'*Wf*Cf)*Cf'*Wf*Yf;
        R = inv(Cf'*Wf*Cf);
        x_est(:,i+1);
        sqrt(R);
        %pause
        
        
    end
    
    X_llse(:,n1) = x_est(1,:)';  
    
end


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

% for n=[2 8]
%     text((n-1)*dt-dt/2,mean(X_llse(n,:)')+.3,['sigma = ',num2str(std(X_llse(n,:)'))])
% end
n = 2;
text((n-1)*dt-dt/2,mean(X_llse(n,:)')-.3,['sigma = ',num2str(std(X_llse(n,:)'))])
n = 8;
text((n-3)*dt,mean(X_llse(n,:)')+.4,['sigma = ',num2str(std(X_llse(n,:)'))])















% 
% 
% %Kalman Filter
% 
%     %Predict movment
%     x_est = F*x_est + B*u;
%     
%     %Predict movement covariance
%     P = F*P*F' + C_u;
% 
%     %Predict sensor covariance
%     %Kalman Gain
%     K = P*H'/(H*P*H'+C_s);
%     
%     %Update state estimate  - may need to change H to match state pattern
%     x_est = x_est+10*K*(y - H*x_est);
%         %real measurments s goes here instead of simulated y
%         %This part should change dynamically based on sensors reporting in
%     
%     
%     %Update covariance estimation
%     P =  (eye(12)-K*H)*P;       %(eye(12)-K*H*P);        % supposed to be (eye(12)-K*H)*P
%     
% 
% 











%%% 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',.2)
% 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