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 = thx;
% 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

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

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