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

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

clear
close all

X = [0;1;-.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 0; 0 1 0 0; 1 0 0 0; 0 1 0 0];
B = [dt.^2/2 0;dt 0;0 dt.^2/2;0 dt];
m = 2;
u = 0;

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

Cd = [0 0 1 0; 0 0 0 1; -1 0 1 dt; 0 -1 0 1];

Q = 10;
% ps = inf;
% vs = inf;

x_est = [0;0;0;0];
Y(:,1) = C*X(:,1) + sigma*randn(4,1);
    
x_est(1) = mean(Y);
ps = sigma/2;
vs = 9999;
R = diag((9999*ones(4,1)).^2);

u = zeros(2,T/dt+1);
uN = Q*randn(1,10);
uN = [uN;0 uN(1:end-1)];

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 + diag(B*Q);
    
    Y(:,i+1) = C*X(:,i+1) + sigma*randn(4,1);
    
    Cf = [C;Cd]; 
    Yf = [Y(:,i+1);X(3:4,i+1);-B(1:2)*u];
    Wf = diag([1./[sigma sigma sigma sigma].^2 1./diag(R)'])
    
    %x_est(:,i+1) = inv(Cf'*Wf*Cf)*Cf'*Wf*Yf
%     R = diag(Cf'*Wf*Cf);
%     ps = sqrt(1./sum(Wf*Cf(:,1)))
%     vs = sqrt(1./sum(Wf*Cf(:,2)))
    x_est(:,i+1) = inv(Cf'*Wf*Cf)*Cf'*Wf*Yf
    
    R = inv(Cf'*Wf*Cf);
    %pause
    
    
end

%%% estimate previous velocity, assume constant acceleration
%%% x'2 + x'1 / 2 = x2 - x1 / dt
%%% x1' = x1'
%%% x1 = x1;

%%% how noisy?
%%% sigma = depth sensor for x2
%%% ps for x1
%%% vs for x'1
%%% 2(ps + vs) for 0 = ...








figure(1)
plot([0:dt:T],X([1 3],:))
%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])








% 
% 
% %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