% Here's what this code illustrates:
% 1) Model of a simple (passive) pendulum.
% 2) Inclusion of "noise" in measurement
%    a) No noise
%    b) Gaussian noise
%    c) encoder discretization "noise" (not Gaussian!!)
% 3) Estimator (aka Observer) design, L, and simulation
% 4) Inclusion of Kalman filtering, to improve estimates

format compact   % for better readability

wn = 5; % natural frequency of pendulum, rad/s
Ac = [0 1; -wn^2 0];  % CONTINUOUS TIME plant model
Bc = [0; 1];
Cc = [1 0];
Dc = [0];
ssc = ss(Ac,Bc,Cc,Dc);  % CONTINUOUS TIME state-space model

pc = eig(Ac) % poles of plant
pc_dominant = min(abs(pc))  % SLOWEST pole of dynamic system; should be wn

rel_est_speed = 5;    % make estimator 2-6x faster than plant dynamics
pe = -rel_est_speed*pc_dominant*[1 1];  %
Lc = acker(Ac',Cc',pe)'  % estimator matrix, "L", for CT plant

%Kc=[0 0]; % no feedback... for now
%Ace = [Ac-Bc*Kc  Bc*Kc
%    zeros(size(Ac)) (Ac-Lc*Cc)];
%Bce = [Bc;
%    zeros(size(Bc))];
%Cce = [Cc zeros(size(Cc))];
%Dce = [Dc];
%ssce = ss(Ace,Bce,Cce,Dce);

% Now, simulate plant as a discrete-time system:
T = 1e-3;
ssd = c2d(ssc,T,'matched');
Ad = ssd.A; Bd = ssd.B; Cd = ssd.C; Dd = ssd.D;
Ld = acker(Ad',Cd',exp(T*pe))'  % match poles, in discrete-time sys

% Case 1: no noise
x0 = [20*pi/180; 0];  % angle and velocity
y0 = 0;               % start with false estimate of state
ndo = 1e3;
t = [0:ndo-1]*T;
x = 0*x0*t;
x(:,1) = x0;  % PLUG IN INITIAL CONDITION
y = 0*t;
xe = 0*x;
ye = 0*y;
u = 0*t;  % no torque; you can modify, if desired...
for n=2:ndo
    % ACTUAL PLANT DYNAMICS:
    x(:,n) = Ad*x(:,n-1) + Bd*u(n-1);
    y(:,n) = Cd*x(:,n-1) + Dd*u(n-1);
    
    % INTERNAL MODEL OF PLANT; ESTIMATOR:
    xe(:,n) = Ad*xe(:,n-1) + Bd*u(n-1) + Ld*(y(:,n-1)-ye(:,n-1));
    ye(:,n) = Cd*xe(:,n-1) + Dd*u(n-1);
end
figure(1); clf
subplot(211)
plot(t,x(1,:)); hold on; grid on
plot(t,xe(1,:),'r--'); xlabel('Time (s)')
ylabel('Angle (rad)')
legend('Actual state','Estimated state')
subplot(212)
plot(t,x(2,:)); hold on; grid on
plot(t,xe(2,:),'r--'); xlabel('Time (s)')
ylabel('Angular velocity (rad/s)')
legend('Actual state','Estimated state')
    

