function RandomWalkKF
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% AS-0.2230 Laboratory Exercises in Control and Automation engineering
% Exercise 16: Sensor fusion with Kalman filter
% Template file for the second preliminary exercise.
% 
% Add your code between the comments in this file. There are two places
% where some code is needed to make this function operational.
% 
% Author: JA
% Date: 17.8.2011
% Revision: 27.9.2011 --> Noise added to first sample
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

close all;
clear all;
figure(1);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Parameters for adjusting the visualization %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
plotRatio = 1;         % Plot: 0 = none, 1 = every, 2 = every other...
ptime = 0.2;           % pause time after each plot in seconds

focus = 1;             % 0 = whole area, 1 = focuses into current point
ymin = 3;              % Parameters for adjusting the size of focus window 
ymax = 3;
xmin = 15;
xmax = 5;


% Allocate memory for all the states and covariances
n = 100; % number of data and measurements generated
Xm=zeros(1,n);
SP=zeros(1,n);

%
% YOU NEED TO INITIALIZE THE MODEL HERE !!!
%

q =              % process noise
r =              % measurement noise
    
Xm(1) =          % initial state
SP(1) =          % initial variance

A =              % Process matrix
H =              % Measurement matrix

%
% END OF THE FIRST PART OF YOUR CODE !!!
%


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Generate the data and measurements %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
[Y,X_r] = generateData(n,q,r);



%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Loop where the filtering is done %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
for i = 2:n
    
    %
    % IMPLEMENT KALMAN FILTER HERE !!!
    %
    

    
        
    % Store latest estimate and covariance
    Xm(i) = 
    SP(i) = 
    
    %
    % YOUR CODE ENDS HERE !!!
    %
    
    %%%%%%%%%%%%%%%%%%
    % Visualize data %
    %%%%%%%%%%%%%%%%%%
    if(~mod(i,plotRatio))
        clf(1)
        if(focus)
            axis([i-xmin i+xmax  X_r(i)-ymin X_r(i)+ymax])
        else
            axis([0 n  min(Y(1:i)) max(Y(1:i))])
        end
        hold on
        plot(i,X_r(i),'ko','MarkerFaceColor','r','MarkerSize',5)
        plot(i,Y(i),'ko','MarkerFaceColor','k','MarkerSize',6)
        plot(i,Xm(i),'ko','MarkerFaceColor','b','MarkerSize',5)
        legend('Real','Meas','Est','Location','Best')
        plot(X_r(1:i),'r.-')
        plot(Y(1:i-1),'k.')
        plot(Xm(1:i),'b.-')
        pause(ptime)
    end
end

%%%%%%%%%%%%%%%%%
% Plot all data %
%%%%%%%%%%%%%%%%%
clf(1)
plot(X_r(:),'r-')
hold on;
plot(Y(:),'k.')
plot(Xm(:),'b-')
legend('Real', 'Meas', 'Est')

end
%%
function [Y, X_r] = generateData(n,q,r)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Generated samples and measurements from one-dimensional
% Gaussian random walk model.
%
% Input:
%       n   = number of samples
%       q   = process noise
%       r   = measurement noise
% Output:
%       Y   = 1 x n vector of measurement samples
%       X_r = 1 x n vector of samples of the process
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

Y = zeros(1,n);
X_r = zeros(1,n);
X_r(1) = 0 + randn(1,1);

for i = 1:n
    if i>1        
        X_r(i) = X_r(i-1) + q*randn(1,1);
    end
    Y(i) = X_r(i) + r*randn(1,1);
end

end