close all;

% sensitivity of cascade of biquads

f = logspace(4, 5, 501);
w = 2*pi*f;

wp = 2*pi * 20.00e3;
ws = 2*pi * 22.05e3;
rp = 0.5;
rs = 50;

[N, Wn] = ellipord(wp, ws, rp, rs, 's');
[b, a] = ellip(N, rp, rs, Wn, 's');
H = tf(b, a);

[z,p,k] = zpkdata(H);
z = z{1};  p = p{1};
Qp = Q(p)
Fp = W(p) / (2*pi) / 1e3
Fz = W(z) / (2*pi) / 1e3

b1 = zpk(z(1:2), p(7:8), k^(1/4));
b2 = zpk(z(3:4), p(5:6), k^(1/4));
b3 = zpk(z(5:6), p(3:4), k^(1/4));
b4 = zpk(z(7:8), p(1:2), k^(1/4));

b1 = b1 / max(abs(freqresp(b1, w)));
b2 = b2 / max(abs(freqresp(b1*b2, w)));
b3 = b3 / max(abs(freqresp(b1*b2*b3, w)));
b4 = b4 / max(abs(freqresp(b1*b2*b3*b4, w)));

% s-plane
pzmap(H);
fixfig;

% response
figure;
bodehz(H, w);

figure;
% bodemaghz(H, w);
hold on;
bodemaghz(b1, w);
bodemaghz(b2, w);
bodemaghz(b3, w);
bodemaghz(b4, w);
legend('Biquad 1', 'Biquad 2', 'Biquad 3', 'Biquad 4', 3);
axis([ min(f) max(f) -50 15]);
fixfig;
hold off;

% ordering
figure;  bodemaghz(b1, w);               title('Biquad 1');                 fixfig;
figure;  bodemaghz(b1*b2, w);            title('Biquads 1, 2');             fixfig;
figure;  bodemaghz(b1*b2*b3, w);         title('Biquads 1, 2, 3');          fixfig;
figure;  bodemaghz(b1*b2*b3*b4, w);      title('Biquads 1, 2, 3, & 4');     fixfig;

% sensitivity

[z4, p4, k4] = zpkdata(b4);
Qp4 = Q(p4{1});     Qp4 = Qp4(1);
Wp4 = W(p4{1});     Wp4 = Wp4(1);
Wz4 = W(z4{1});     Wz4 = Wz4(1);

Wz4 = 0.99*Wz4;
Wp4 = 1.01*Wp4;         % increase wp of biquad 4 by 0.5%

s = tf('s');
b4changed = abs(evalfr(b4, 0)) * (1 + (s/Wz4)^2) / ( 1 + s/(Qp4*Wp4) + (s/Wp4)^2);

figure;
bodemaghz(b4, w);
hold on;
bodemaghz(b4changed, w);
fixfig;

figure;
bodemaghz(H, w);
hold on;
bodemaghz(b1*b2*b3*b4changed, w);
fixfig;
hold off;



