Untitled

%% comp3 2.3 Kalman filter for prediction
addpath '/Users/noahakesson/MATLAB-Drive/Comp2/Tidserie/functions'
addpath '/Users/noahakesson/MATLAB-Drive/Comp2/Tidserie/data'
close all
clear all

%%
rng (0);
N = 10000;
ee = 0.1*randn(N, 1 ) ;
A0 = [1 -0.8 0.2 ];
y = filter(1 , A0 , ee );
Re = [ 1e-6 0 ; 0 1e-6];
Rw = 0.1 ;
% Set some initialvalues
A = eye(2);

Rxx_1 = 10*eye ( 2 ) ;              % Initialstate variance
xtt_1 = [ 0 0 ]';                   % Initialstatevalues %xtt_1 --> x_{t|t-1}
%xt = zeros(2,N);                  % x_{t|t} initial state
% Vectors to store values in
Xsave = zeros( 2 ,N) ;              % Stored states
ehat = zeros( 1 ,N) ;               % Prediction residual
yt1 = zeros( 1 ,N) ;                % One s t e p p r e d i c t i o n
yt2 = zeros( 1 ,N) ;                % Two s t e p p r e d i c t i o n

% The f i l t e r use data up to t ime t-1 to predictvalue at t,
% then update using the prediction error. Why do we start
% from t=3? Why stop at N-2?

for t=3:N-2

xtt_1 = A*Xsave(:,t-1);                 % x_{t|t-1} = A x_{t-1|t-1}
Ct = [ -y(t-1) -y(t-2) ] ;  %C_{t|t-1}
yhat(t) = Ct*xtt_1;  %y_{t|t-1} = Ct*x_{t|t-1}
ehat(t) = y(t)-yhat(t);  % e_t = y_t - y_{t|t-1}

% Update
Ryy = Ct*Rxx_1*Ct' + Rw;            % R^{yy}_{t|t-1} = Ct*R_{t|t-1}^{x,x} + Rw
Kt = Rxx_1*Ct'/Ryy;                 % K_t = R^{x,x}_{t|t-1} C^T inv( R_{t|t-1}^{y,y} )
xt_t = xtt_1 + Kt*(ehat(t));        % x_{t|t} = x_{t|t-1} + K_t ( y_t - Cx_{t|t-1} ) 
Rxx = Rxx_1 - Kt*Ryy*Kt';           % R{xx}_{t|t} = R^{x,x}_{t|t-1} - K_t R_{t|t-1}^{y,y} K_t^T 

% Predict the nextstate
%xt_t1 = A*xt_t;                     % x_{t+1|t} HÄR KAN VARA EN B*U_t term om man har input???
Rxx_1 = A*Rxx*A' + Re;              % R^{x,x}_{t+1|t} = A R^{x,x}_{t|t} A^T + Re

% Form 2 stepprediction . Ignore this part at first .
Ct1 = [ -y(t) -y(t-1) ] ; 
yt1(t+1) = Ct1*xt_t; 
Ct2 = [ -yt1(t+1) -y(t) ]; 
yt2 (t+2) = Ct2*A*xt_t; 
% % Store the statevector
Xsave(:, t) = xt_t;

yhatk(t+2) = yt2(t+2);
end

%% Examine the estimated parameters.
k=2;
figure
Q0 = [A0(2) A0(3)];                       % These are the true parameters we seek.
plot( Xsave' )
for k0=1:length(Q0)
    line([1 N], [Q0(k0) Q0(k0)], 'Color','red','LineStyle',':')
end
title(sprintf('Estimated parameters, with Re = %7.6f and Rw = %4.3f', Re, Rw))
xlabel('Time')
ylim([-1.5 1.5])
xlim([1 N-k])

%% Plot
figure
plot(Xsave(1,:))
hold on
plot(Xsave(2,:))
legend('a1', 'a2', 'Location','SW')

figure
plot(ehat)
title('residual')

sumsqr(ehat)


%% Show the k-step prediction. 
k=2;
figure
plot( [y(1:N-k) yhatk(k+1:N)] )
title( sprintf('%i-step prediction using the Kalman filter (shifted %i steps)', k, k) )
xlabel('Time')
legend('Realisation', 'Kalman estimate', 'Location','SW')
xlim([1 N-k])