% Matlab code for simulation of x(k+1) = ad x(k) + bd (u(k) + w(k), y(k) = x(k) + n(k)
% and least-squares system identification
% Prabir Barooah

clear all;

% parameters of the continuous time model
a = -0.2;
b = 50;

T = 1e-2; %sampling period, in second;

forcing_freq = 5; %in Hz

% parameters of the discrete time model
ad = exp(a*T);
bd = b*(ad-1)/a;

%initial condition
xinit = 0.0;


N = 3*round(1/T); %number of data points collected in a single experiment
time_vec = [0:1:N-1]'*T;

std_dev_w = 0.1;
std_dev_n = 0.5;

u_vec = sin(2*pi*forcing_freq*time_vec); %input - unit amplitude sinusoid
u_actual = u_vec + std_dev_w*randn(length(u_vec),1);


z = zeros(N,1);
H = zeros(N,2);

%intialize
x_prev = xinit;

for ii=1:N

    x_current = ad * x_prev + bd*u_actual(ii);
    
    % measured_output
    y_prev = x_prev + std_dev_n*randn(1);
    y_current = x_current + std_dev_n*randn(1);

    % collect data for least- squares parameter estimate
    z(ii,1) = y_current;
    H(ii,[1 2]) = [y_prev u_vec(ii)];

    % update state
    x_prev = x_current;
end;

figure(1);hold off
plot(time_vec,z,'r',time_vec,u_vec,'b-')
%'Batch' least squares estimate
theta_hat = H\z