% x = [theta; dtheta]
% I'm assuming: tau = .1 sec,
% TF is K/(tau*s^2 + s), where K is about 12.37.
% The model ignores the static friction, but it's still a starting point...

A = [0 1; 0 -10]; B = [0 12.37]'; C = [1 0]; D = 0; % CT dynamics
ss1 = ss(A,B,C,D);
T = .001; % sample time
ss1z = c2d(ss1,T,'zoh'); % DT dynamics
Az = ss1z.A; Bz = ss1z.B; Cz = ss1z.C; Dz = ss1z.D;
K = [10 0]; % Kp gain for proportional controller
poles_con = eig(A-B*K)
wn_poles = abs(poles_con);
poles_est = -4*wn_poles;  % could also be underdamped, but let's try crit damp first...
Lp = acker(ss1z.a',ss1z.c',exp(poles_est*T))'  % acker, b/c repeated poles

Xinit = [0;0]; % True initial condition
Xdes = [90*pi/180; 0]; % step response test...

Xtraj = zeros(2,2000);  % pre-allocate
Xtraj(:,1) = Xinit; % initialize at t=0
Xest = Xtraj; % initialize the estimate
y = 0*Xtraj(1,:); % sensor output from encoder (discretized)

for n=2:length(Xtraj(1,:))
    u = K*(Xdes-Xtraj(:,n-1)); % control output
    Xtraj(:,n) = Az*Xtraj(:,n-1) + Bz*u; % update true dynamics
    y(n) = (pi/180)*round((180/pi)*C*Xtraj(:,n)); % 1 degree resolution encoder
    yhat = C*(Az*Xest(:,n-1) + Bz*u); % what we EXPECT to sense...
    Xest(:,n) = Az*Xest(:,n-1) + Bz*u + Lp*(y(n)-yhat); % update estimate
end
t = T*[0:n-1];    % (for plotting)

DS = 180/pi; % "degree scaling" for plotting

figure(1); clf
plot(t,y); hold on
plot(t,DS*Xtraj(1,:),'r');
plot(t,DS*Xest(1,:),'k');
title('POSITION')
nsamp = 25; % I think you guys used 25?
tf_filt = tf([1],[nsamp*T 1]);
tf_filt_z = c2d(tf_filt,T,'tustin');
tn = tf_filt_z.num{:};
td = tf_filt_z.den{:};
yf = filter(tn,td,y);
plot(t,DS*yf,'g')
legend('Encoder','Actual','Estimator','Low-pass Filter')
xlabel('Time (s)'); ylabel('Angle (deg)')

figure(2); clf
dy = (1/T)*(y(2:end)-y(1:end-1));
plot(t(2:end),DS*dy); hold on
plot(t,DS*Xtraj(2,:),'r');
plot(t,DS*Xest(2,:),'k');
title('VELOCITY')
dyf = filter(tn,td,dy);
plot(t(2:end),DS*dyf,'g')
legend('Encoder','Actual','Estimator','Low-pass filter')
xlabel('Time (s)'); ylabel('Angular velocity (deg/s)')

