L = 5;                                      % loop order
M = 64;                                     % oversampling ratio
B = 20e3;                                   % signal bandwidth
fN = 44.1e3;                                % nyquist rate
fs = M * fN;                                % sampling frequency

fb = linspace(0, fs/M/2, 100);              % baseband frequencies
f  = linspace(0, fs/2, 100);                % filter frequencies
flog = logspace(log10(fs)-4, log10(fs/2), 100);

% design NTF
Rstop = 80;                                 % stop-band attenuation ... reduce if design is not stable
[b,a] = cheby2(L, Rstop, 1/M, 'high');
b = b/b(1);                                 % normalize (for causality)
NTF = filt(b, a, 1/fs);

% check for stability (should be < 1.5, see DS Data converters p. 152)
[mag] = bode(NTF, pi*fs)

% plot the NTF
bebfigure(1);
m = freqresp(NTF, 2*pi*flog);  m = 20*log10(abs(m(1,:)));
semilogx(flog, m, 'linewidth', 2);
hold on;
plot([ B B ], [min(m) max(m)], 'r:', 'linewidth', 2);
xlabel('Frequency   [Hz]');
ylabel('NTF  [dB]');
axis([min(flog) max(flog)  -Rstop-20 20]);
hold off;

% loop filter
H = inv(NTF) - filt(1, 1, 1/fs);

% check that the impulse response starts with 0 (causality)
y = impulse(H);
y = y(1)

% plot loop filter H
bebfigure(2);
m = freqresp(H, 2*pi*flog);  m = 20*log10(abs(m(1,:)));
semilogx(flog, m, 'linewidth', 2);
hold on;
plot([ B B ], [min(m) max(m)], 'r:', 'linewidth', 2);
xlabel('Frequency   [Hz]');
ylabel('Loopfilter  H  [dB]');
axis([min(flog) max(flog) -20 Rstop+20]);
hold off;

% pzmap
bebfigure(3);
pzmap(H);

% root locus
bebfigure(4);
R = rlocus(H);
plot(R', 'linewidth', 2);
hold on;
R = rlocus(H, 0);       plot(R', 'o');
R = rlocus(H, Inf);     plot(R', 'x');
R = rlocus(H, 0.55);    plot(R', 's');
t = -0.5:0.01:0.5;
plot(cos(t), sin(t), 'k');
axis([ 0.8 1.1 -0.4 0.4 ]);
xlabel('\sigma - Axis');
ylabel('j\omega - Axis');
legend('root locus', '', '', '', '', 'K = 0', 'K = infinity', 'K = 0.55', -1);
hold off;

% stable root locuse
bebfigure(5);
R = rlocus(H, [ linspace(0.55, 5, 1000) Inf ]);
plot(R', 'linewidth', 2);
hold on;
R = rlocus(H, 0);       plot(R', 'o');
R = rlocus(H, Inf);     plot(R', 'x');
R = rlocus(H, 0.55);    plot(R', 's');
t = -0.5:0.01:0.5;
plot(cos(t), sin(t), 'k');
axis([ 0.8 1.05 -0.4 0.4 ]);
title('Root Locus for K > 0.55');
xlabel('\sigma - Axis');
ylabel('j\omega - Axis');
hold off;

% filter with rounded coefficients
a1=1;
a2=1/2;
a3=1/4;
a4=1/8;
a5=1/8;

k1=1;
k2=1;
k3=1/2;
k4=1/4;
k5=1/8;

b1=1/1024;
b2=1/16-1/64;

N  =  z^4 * a1 ...
    + z^3 * (-4*a1 + a2*k2) ...
    + z^2 * (a1*k2*b1*k3 + a1*k4*b2*k5 + 6*a1+a3*k2*k3 - 3*a2*k2) ...
    + z   * (-4*a1 - 2*a3*k2*k3 + 3*a2*k2 + a2*k2*k4*b2*k5 - 2*a1*k4*b2*k5 + a4*k4*k2*k3 - 2*a1*k2*b1*k3) ...
    +       (a1 -a2*k2*k4*b2*k5 + a3*k2*k3 + a1*k4*b2*k5*k2*b1*k3 + a3*k2*k3*k4*b2*k5 - a4*k4*k2*k3 + a1*k4*b2*k5 + a5*k4*k2*k3*k5 + a1*k2*b1*k3 - a2*k2);

D  =  z^5 ...
    + z^4 * (-5) ...
    + z^3 * (k4*b2*k5 + 10 + k2*b1*k3) ...
    + z^2 * (-10 - 3*k2*b1*k3 - 3*k4*b2*k5) ...
    + z   * (5 + 3*k2*b1*k3 + 3*k4*b2*k5 + k4*b2*k5*k2*b1*k3) ...
    +       (-1 - k2*b1*k3 - k4*b2*k5 - k4*b2*k5*k2*b1*k3);

H = N/D;