Untitled

%% comp3 2.2 Kalman filtering of time series
addpath '/Users/noahakesson/MATLAB-Drive/Comp2/Tidserie/functions'
addpath '/Users/noahakesson/MATLAB-Drive/Comp2/Tidserie/data'
close all
clear all

%% load data
load('tar2.dat')
load('thx.dat')

%% plot data
x = 0:499;
figure; 
subplot(211); 
plot(x, thx(:,1))
hold on
%plot(x, thx(2,:))
ylabel('Parameter trajectories?')
title('Measured signals')
%legend('Input signal', 'Prediction starts','Location','SW')
subplot(212); 
plot(x, tar2 ); 
ylabel('AR(2)')
xlabel('Time')
%legend('Output signal', 'Prediction starts','Location','SW')

%%
% X = recursiveAR( 2 ) ;
% X.ForgettingFactor = 1 ;
% X.InitialA = [ 1 0 0 ] ;
% for kk=1:length( tar2 )
%     [Aest(kk, : ) , yhat( kk )] = step(X, tar2( kk )); %Aest is the estimated parameter,yhat is estimate of y_t based A(z) poly
% end
% 
% figure
% plot(Aest)
% 
% %% KÖR INTE???
% % Simulate an AR(2) process.
% rng(0)                                          % Set the seed (just done for the lecture!)
% N  = 500;
% A0_start = -0.95;
% A0_stop  = -0.45;
% A1 = linspace( A0_start,A0_start, N);
% A2 = linspace( A0_start,A0_start, N)
% %A1(N/2:end) = A0_stop;                         % Abruptly change a_1.
% y = zeros(N,1);
% e = randn( N, 1 );
% for k=3:N                                       % Implement filter by hand to allow a1 to change.
%     y(k) = e(k) - A1(k)*y(k-1) - A2(k)*y(k-2);
% end
%%
N=length(tar2)
y = tar2;
% Define the state space equations .
A = [ 1 0 ; 0 1 ] ;
Re = [ 0.004 0 ; 0 0 ] ;            % State covariance matrix
Rw = 1.25;                           % Observation variance
% Set some initialvalues
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) ;               % Pr e d i c t i on r e s i d u a l
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 = [ ? ? ] ; 
% yt1 ( t+1) = ? 
% Ct2 = [ ? ? ] ; 
% yt2 ( t+2) = ? 
% % Store the statevector
Xsave(:, t) = xt_t;

end

%% Examine the estimated parameters.
Q0 = [thx(:,1) thx(:,2)];                       % These are the true parameters we seek.
figure
plot( Xsave' )
hold on
plot(thx(:,1), 'Color','red','LineStyle',':')
hold on
plot(thx(:,2), 'Color','red','LineStyle',':')
title(sprintf('Estimated parameters, with Re = %7.6f and Rw = %4.3f', Re, Rw))
xlabel('Time')
ylim([-2 2])

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