kalmanfilter

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

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
% 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
% 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 p r e d i c t v a lue 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)  ]; 
yhat(t) = C*x_t1;                           % One-step prediction, \hat{y}_{t|t-1}.
ehat( t ) = y(t)-yhat(t);                     % One-step prediction error, \hat{e}_t = y_t - \hat{y}_{t|t-1}

% update
Ryy = C*Rx_t1*C' + Rw;                       % R_{t|t-1}^{y,y} = C R_{t|t-1}^{x,x} + Rw
Kt = Kt = Rx_t1*C'/Ry;                           % K_t = R^{x,x}_{t|t-1} C^T inv( R_{t|t-1}^{y,y} ) 
xt_t = x_t1 + Kt*( h_et(t) );            % x_{t|t}= x_{t|t-1} + K_t ( y_t - Cx_{t|t-1} )
Rxx = Rx_t1 - Kt*Ry*Kt';                  % R^{x,x}_{t|t} = R^{x,x}_{t|t-1} - K_t R_{t|t-1}^{y,y} K_t^T

%predict the next state
x_t1 = A*xt_t(:,t-1);                         % x_{t|t-1} = A x_{t-1|t-1}
Rxx_1 = A*Rx_t*A' + Re;                     % R^{x,x}_{t+1|t} = A R^{x,x}_{t|t} A^T + Re

C1 = [   -y(t) -y(t-1) ];           % C_{t+1|t}
y2 = Ck*xt_t(:,t);                            % \hat{y}_{t+1|t} = C_{t+1|t} A x_{t|t}