%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%  This m-file shows how to use D-T convolution to explore how to use 
%%  two LTI D-T systems together with some 
%%  "exception processing" to convert measurements of a motor's angular   
%%  position into an estimate of its rotational speed                     
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

close all  % closes all open figure windows

% Create time samples spaced 1 ms apart
del_t=0.001;
t=0:del_t:3;

%% compute ramp function with slope of 5*pi radians/sec
%%  simulates angular position of motor shaft spinning at a constant rate
theta=5*pi*t;  
%% use some matlab "tricks" to simulate the fact that
%% measured angle "wraps" around to always lie between 0 and 2*pi
theta_m=angle(exp(j*theta));

figure(1)
plot(t,theta_m,'o-')
xlabel('time (s)')
ylabel('measured angular position (radians)')

%  specify impulse response of D-T system that computes 
%%  the time derivative of the input
h_1=[-3 16 -36 48 -25]/(12*del_t);

%%% Compute D-T system's output using convolution
x=theta_m;   % just rename position measurements to "typical" input symbol
y=conv(x,h_1);

figure(2)
plot(t,y(1:length(t)),'o')   %% convolution causes output to be spread to be longer than input
                         %%  so plot a shortened version of the output
%%% Note the large "spurious" values in the output... these are due to the artificial 
%% discontinuity as the position sensor switches from -pi to pi
xlabel('time (s)')
ylabel('measured angular rate (radians/second)')


%% we can do some computer-based "exception processing" to get rid of those 
%%  by throwing away output values that are bigger than we would expect the 
%% rotational rate to be

I=find(abs(y)>500);
y(I)=0;


%%%% Now plot the "cleaned-up" version
figure(3)
plot(t,y(1:length(t)),'o')   %% convolution causes output to be spread to be longer than input
xlabel('time (s)')
ylabel('measured angular rate (radians/second)')


y2a=conv(y,ones(1,100))/100;   % do sliding average of 100 pts to reduce effect of inserted zeros

figure(4)
plot(t,y2a(1:length(t)),'o')   %% convolution causes output to be spread to be longer than input
xlabel('time (s)')
ylabel('measured angular rate (radians/second)')

hold on

%% Try a longer "all ones" impulse response to improve the accuracy through more "averaging"
y2b=conv(y,ones(1,1000))/1000;   % do sliding average of 1000 pts to reduce effect of inserted zeros
figure(4)
plot(t,y2b(1:length(t)),'rx')   %% convolution causes output to be spread to be longer than input
xlabel('time (s)')
ylabel('measured angular rate (radians/second)')
hold off


%%% Now explore what happens when the angular rate changes abruptly....

theta1=5*pi*t(1:1500);  %% first rate is 5*pi rad/sec
theta2=7*pi*t(1501:3001);  %% second rate is 7*pi rad/sec
theta2=(theta2-theta2(1))+t(1501)*5*pi;
theta=[theta1 theta2];
theta_m=angle(exp(j*theta));

figure(5)
plot(t,theta_m,'o-')
xlabel('time (s)')
ylabel('measured angular position (radians)')


x=theta_m;   % just rename position measurements to "typical" input symbol
y=conv(x,h_1);

I=find(abs(y)>500);
y(I)=0;

y2a=conv(y,ones(1,100)/100);   % do sliding average of 100 pts to reduce effect of inserted zeros

figure(6)
plot(t,y2a(1:length(t)),'o')   %% convolution causes output to be spread to be longer than input
xlabel('time (s)')
ylabel('measured angular rate (radians/second)')

hold on

%% Try a longer "all ones" impulse response to improve the accuracy through more "averaging"
y2b=conv(y,ones(1,1000)/1000);   % do sliding average of 100 pts to reduce effect of inserted zeros
plot(t,y2b(1:length(t)),'rx')   %% convolution causes output to be spread to be longer than input
xlabel('time (s)')
ylabel('measured angular rate (radians/second)')
hold off