%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%% 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