Untitled

%% comp3_24
close all
clear all

N=500;


[P,u] = markov(N, 7/8, 7/8);

var_ee = 1;
var_ww = 4;
b=20;

rng (0);
N = 10000;
ee = var_ee*randn(N, 1 ) ;
ww = var_ww*rand(N, 1);

A0 = 1;
A1 = linspace( A0,A0, N);
y = zeros(N,1);
Re = [ 1e-6 0 ; 0 1e-6];
Rw = 0.1;

for k=2:N                                       % Implement filter by hand to allow a1 to change.
    y(k) = ee(k) + ww(k) + b*u(k) + A0*y(k-1);
end

% Plot realisation.
figure
plot(y)
title( 'Realisation of an hittepåsakmojäng-process' )
ylabel('Amplitude')
xlabel('Time')
xlim([1 N])

figure
subplot(2,1,1)
plot(y(end-100-2:end-2))
subplot(2,1,2)
plot(u(end-100-2:end-2))


%%
%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) ;               % 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

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}

xtt_1 = A.*Xsave(:,t-1) + b.*u(:,t-1);                 % x_{t|t-1} = A x_{t-1|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;

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')

figure
plot(ehat)
title('residual')

sumsqr(ehat)