clear variables;
delete(findall(0,'Type','figure'));
clc;
%% Plant Coefficients
t = 1:10000;
a = length(t);
a1 = -0.9646;
a2 = -0.192;
a3 =  0.169;
a4 = -0.01244;
b1 = -1.23;
b2 =  1.547;
b3 =  0.1055;
b4 =  2.812;

%% Generating Reference Control-Input
r = zeros(1,10000);
r(   1:1000)  = rand(1,1000);
r(   1:1000)  = 0.001*((r(1:1000)-0.5)*2);%(u(   1:500)-0.5)*0.1 
r( 500:1000)  = r( 500:1000)+1.5;
r(1001:5500)  = r(1001:5500)+sin(0.0028*t(1:4500))+1.5;
r(5500:6000)  = r(5500:6000)+0;
r(6001:7000)  = r(6001:7000)+3;
r(7001:7500)  = r(7001:7500)+0;
r(8001:9000)  = r(8001:9000)+1.5;
r(9001:10000) = r(9001:10000)+0;
y(   1:10000) = 0;

%% Recursive Init
theta0 = [a1;a2;a3;a4;b1;b2;b3;b4];
theta = zeros(8,a);
theta(:,4) = theta0;
lambda = 0.99;
P0 = [1000 0 0 0 0 0 0 0;
      0 1000 0 0 0 0 0 0;
      0 0 1000 0 0 0 0 0;
      0 0 0 1000 0 0 0 0;
      0 0 0 0 1000 0 0 0;
      0 0 0 0 0 1000 0 0;
      0 0 0 0 0 0 1000 0;
      0 0 0 0 0 0 0 1000];
P     = zeros(8,8,a);
P(:,:,4) = P0;
phi   = zeros(a,8);
K     = zeros(8,a);

%% Initialise Variables/States
N_bar = ones(1,a);      % Precompensator
f_bar = zeros(a,1);     % Value from Feedback Controller
v     = zeros(a,1);     % Value after Precompensator
u     = zeros(a,1);     % System Input
x_bar = zeros(4,a);     % Feedback Controller Input

for i=5:a;
    v(i) = N_bar(i-1) * r(i);
    u(i) = v(i) - f_bar(i-1);
    %% Data Generation
    y(i)   = a1*y(i-1)+a2*y(i-2)+a3*y(i-3)+a4*y(i-4)...
                +b1*u(i-1)+b2*u(i-2)+b3*u(i-3)+b4*u(i-4);
    y(i)   = y(i) + 0.01*((rand()-0.5)*2);   
    %% Recursive Analysis
    phi(i,:)    = [y(i-1) y(i-2) y(i-3) y(i-4) u(i-1) u(i-2) u(i-3) u(i-4)];
    K(:,i)      = P(:,:,i-1)*phi(i,:)'/(lambda+phi(i,:)*P(:,:,i-1)*phi(i,:)');
    P(:,:,i)    = ((eye(8,8)-K(:,i)*phi(i,:))*P(:,:,i-1))/lambda;
    theta(:,i)  = theta(:,i-1)+K(:,i)*(y(i)-phi(i,:)*theta(:,i-1));
    %% StateSpace Modelled System
    [Ao, Bo, Co, Do] = tf2ss([theta(5,i) theta(6,i) theta(7,i) theta(8,i)],...
                             [1 -theta(1,i) -theta(2,i) -theta(3,i) -theta(4,i)]);
    %% Feedback Controller
    pls = [-0.11 -0.12 -0.13 -0.14];
    F = acker(Ao,Bo,pls);
    %% Observer
    L = acker(Ao',Co',[0.0196 0.0196 0.0196 0.0196])';
    %% Calculate Next State
    x_bar(:,i) = Bo*(u(i-1))+Ao*x_bar(:,i-1)+L*(y(i-1)-Co*x_bar(:,i-1)); 
    f_bar(i) = F * x_bar(:,i);
	%% Precompensator
    N_bar(i) = inv(-Co*inv(Ao-Bo*F)*Bo);
end
figure(1);
subplot(4,1,1)
    plot(theta');
    grid;
    title('Model Coefficients'), 
    axis([0 a min(min(theta)) max(max(theta))]),
    legend('a1','a2','a3','a4','b1','b2','b3','b4');
subplot(4,1,2:3)
    plot(y,'k');
    hold on;
    plot(r,'r');
    hold off;
    axis([0 a -1 5]);
    grid;
    title('Feedbackcontroller'), 
    legend('y','r');
subplot(4,1,4)
    plot(u), grid on;
    legend('Ctrl Output');
    axis([0 a -2 2]);
    
% 	%%
%     y_a = [y1(i) y1(i-1) y1(i-2) y1(i-3) y1(i-4)];
%     u_a = [u1(i) u1(i-1) u1(i-2) u1(i-3) u1(i-4)];
%     u1(i) = func_ObsFedCtrl(i,theta(:,i),5,y_a,u_a,pls);