% File name: main_rols_ex.m
% Example:
% Implementation of a Reqursive Ordinary Least Squares (ROLS) algorithm
% for ARX model parameter estimation.

clear all
clc

a1=-0.7; a2=1.5; b1=0.25; b2=0.625;
A=[0 1;a1 a2]; B=[b1;b2]; D=[1,0];

N=200;
rand('state',0), randn('seed',0)
%u=randn(N,1);
u=prbs1(N,10,40);

randn('seed',0)

M=1;
for i=1:M                    % Do the simulations M=5 times. Monte carlo simulation
y=dsrsim(A,B,D,0,u);
y=y+randn(N,1)*1.2;

% Input and output data in u and y

Th=zeros(N,4);                         % Array to hold parameter estimates
P=1000*eye(4); th=[0;0;0;0]; Lam=0.998;   % Initial values

%Lam=1;
lf=1;
for t=3:N
    Phi=[y(t-1);y(t-2);u(t-1);u(t-2)]; % Varphi_t matrix at time t.
        
    ybar=Phi'*th;                      % Predikted output at time t.
    
    Pi=inv(P);                         % Updating the covariance matrix, P_t.
    Pi=lf*Pi+Phi*Lam*Phi'; P=inv(Pi);
           
    K=P*Phi*Lam;                       % Kalman gain, K_t.                   
    
    th=th+K*(y(t)-Phi'*th);            % Parameter estimates at time, t.
    
    th
    
    Th(t,:)=th';                       % Store the parameter estimates.
end    
Tm(i,:)=th';         % Store the M=5 set of parameter estimates..
end
    
th
th0=[a2;a1;b1;b2-a2*b1]

figure(1)
subplot(411), plot(Th(:,1)), ylabel('a_2'), title('ROLS example')
subplot(412), plot(Th(:,2)), ylabel('a_1')
subplot(413), plot(Th(:,3)), ylabel('b_1')
subplot(414), plot(Th(:,4)), ylabel('b_2-a_2b_1')
xlabel('Diskret tid: t')

figure(2)
subplot(211), plot(u), ylabel('u_k'), title('ROLS_example')
subplot(212), plot(y), ylabel('y_k')
xlabel('Diskret tid: t')