% AS-84.3128 Assignment 1. Problem P2
% Make calculations and run the file
% u = input
% x = state
% y = measurement

% data is read from the file P2_data
load P2_data

% Fill up the spaces marked with ? with equations. Make sure that the 
% solutions are reasonable. Insert ready code to solutions pdf-file, explain the
% different equations and add figures.

% The system given in the problem is x(k+1)= Fx(k)+Gu(k)+v(k),
% y(k)=Hx(k)+w(k) and Q=E[vv'],R=E[ww']

F = ?    %(5.2.1-1)
H = ?    %(5.2.1-3)
G = ?    %(5.2.1-1)
Q = ?    %(5.2.1-2)
R = ?    %(5.2.1-4)

%% The Kalman Filter
% The initial conditions given in the problem are:
% State prediction covariance
Ppri(:,:,1) = ?;
% State prediction:
xpri(:,1) = ?;

% Implement Kalman Filter using Figure 5.2.4-1 "The Workhorse of Estimation" 
% (is also in the Equation collection!).
for k=1:100
   
    %% State update (also called measurement update)
    
    % Updating the state covariance 
    % Innovation covariance
    S(:,:,k) = ?
    % Filter gain
    W(:,:,k) = ?
    % Updated state covariance
    Ppos(:,:,k) = ?
    
    % Updating the state estimate
    % Measurement prediction
    ypri(:,k) = ?;
    % Measurement residual
    v(:,k) = ?;
    % Updated state estimate
    xpos(:,k) = ?
    
    
    %% Prediction
    % State prediction
    xpri(:,k+1) = ?
    % State prediction covariance
    Ppri(:,:,k+1) = ?
    
end

figure(1); clf
% Plotting the input, simulated state and estimated state
subplot(2,1,1)
% input
plot(u,'k-');
hold on
plot(x(1,1:100),'r--')
plot(x(2,1:100),'m--')
plot(xpos(1,1:100), 'c-')
plot(xpos(2,1:100), 'b-')
legend('Input', 'State x1','State x2',...
    'Estimate state x1','Estimatedstate x2',...
    'Location','NorthEast');

% Simulated and predicted measurements
subplot(2,1,2)
plot(y,'g-')
hold on
plot(ypri, 'r-')
legend('Measurement y','Predicted measurement y',...
    'Location','NorthEast');


figure(2); clf
% State covariances into two dimensional data (value vs. mesurement)
for i = 1:size(Ppri,3)
    cov_1(i) = Ppri(1,1,i);    
end

for i = 1:size(Ppri,3)
    cov_2(i) = Ppri(2,2,i);    
end
% Plotting state covariances
subplot(2,1,1)
plot(cov_1,'b-')
hold on
plot(cov_2,'r-')
legend('State covariance x1','State covariance x2',...
    'Location','NorthEast');


subplot(2,1,2)
% Plotting measurement vs. predicted measurement difference
plot(y-ypri,'r.')
hold on
diff_mean = ones(size(y)) * mean(y-ypri);
plot(diff_mean,'b-')

legend( 'measurement vs. predicted measurement', 'difference mean',...
            'Location','NorthEast');

        
figure(3); clf
% Plotting state x1 vs. estimated state
subplot(2,1,1)
plot(x(1,1:100)-xpos(1,1:100),'g.')
hold on
diff_mean = ones(size(y)) * mean(x(1,1:100)-xpos(1,:));
plot(diff_mean,'k-')
legend( 'State x1 vs. estimated', 'Difference mean',...
            'Location','NorthEast');

% Plotting state x2 vs. estimated state
subplot(2,1,2)
plot(x(2,1:100)-xpos(2,1:100),'c.')
hold on
diff_mean = ones(size(y)) * mean(x(2,1:100)-xpos(2,1:100));
plot(diff_mean,'r-')
legend( 'State x2 vs. estimated', 'Difference mean',...
            'Location','NorthEast');
        
%clear all the variables from work space
clear all