% Verify the numerical first-order differentiator 
% with filters of 2nd, 4th, 8th, and 12th order.
%
% Chapter 2 'Data Gathering'
% "Flight Vehicle System Identification - A Time Domain Methodology"
% Author: Ravindra V. Jategaonkar
% published by AIAA, Reston, VA 20191, USA

clear all

% Generate input signal (sine wave)
dt = 0.01;                           % sampling time
w  = [0:dt:2*pi]';
xsinw = sin(w);

% Analytical differentiation of input 
xcosw = cos(w);

% Generate time derivative by numerical differentiation
Nzi    = 1;
Ndata1 = size(w);
izhf   = Ndata1;

% Filter of second order
xDot2 = xsinw;
xDot2 = ndiff_Filter02(xDot2, Nzi, izhf, dt); 

% Filter of fourth order
xDot4 = xsinw;
xDot4 = ndiff_Filter04(xDot4, Nzi, izhf, dt); 

% Filter of eigth order
xDot8 = xsinw;
xDot8 = ndiff_Filter08(xDot8, Nzi, izhf, dt); 

% Filter of twelfth order (New)
xDot12 = xsinw;
xDot12 = ndiff_Filter12(xDot12, Nzi, izhf, dt); 

% Plot input and its derivatives
plot(w,xsinw,'m',  w,xcosw,'b',  w,xDot2,'r',  w,xDot4,'k', w,xDot4,'g', w,xDot12,'c');
axis([0 7, -1 1]); grid;
legend('Input', 'Analytic diff', 'DiffFilter02', 'DiffFilter04',...
                                 'DiffFilter08', 'DiffFilter12', 1);

% Error between analytical and numerical differentiation
sum02 = mean(xcosw-xDot2);
sum04 = mean(xcosw-xDot4);
sum08 = mean(xcosw-xDot8);
sum12 = mean(xcosw-xDot12);