% lsq.m
%
% Least-squares parameter estimation for an RLC circuit.
% Evaluate theoretical frequency response, and test least-squares solution.
% Also find w0, B, and Q from LS coefficients X, Y, and Z.

% Add noise to frequency response to check sensitivity.
% The way to fix the LS is to omit the data points where H is small.
% At those points, the error is large, so the fit is much affected
% by the error.

% NOTE: Since take reciprocal, fit is better at small |H| values.

R = 146;
L = 100e-3;
C = 15.8e-9;
r = 105;

N = 10;    % Number of points to evaluate frequency response

% Add noise that is uniformly distributed in [-Namp/2, +Namp/2]
Namp = .01;

f = linspace(3000, 5000, N);  % Choose N points linearly spaced
w = 2*pi*f;
h = R ./ ((R+r) + j*(w*L - 1./(w*C)));

nse = Namp*(rand(size(h))-0.5);
H = abs(h) + nse;

% Just pick out a portion of the curve to fit
z = find(H > 0.1);
N = length(z);
f = f(z);
w = w(z);
h = h(z);
H = H(z);

% Normalize the frequency values to avoid computational problems.

w1 = sqrt(w(1)*w(N));	% Frequency to normalize by
v = w/w1;

M = [ sum(v.^(-4)), sum(v.^(-2)), N;
sum(v.^(-2)), N, sum(v.^(2));
N, sum(v.^(2)), sum(v.^(4)) ];

G = 1./(H.^2); 
b = [sum(v.^(-2).*G); sum(G); sum(v.^(2).*G)];

x = M \ b;

Xp = x(1);
Yp = x(2);
Zp = x(3);

X = x(1) * (w1^2);
Y = x(2);
Z = x(3) * 1/(w1^2);

Hls = w ./ sqrt( X + Y*w.^2 + Z*w.^4 );

% Evaluate the frequency responses at more frequencies in order
% to draw smooth lines on plot

f1 = linspace(3000, 5000, 100);
w1 = 2*pi*f1;
H1 = abs(R ./ ((R+r) + j*(w1*L - 1./(w1*C))));
Hls1 = w1 ./ sqrt( X + Y*w1.^2 + Z*w1.^4 );

figure(1)
plot(f, H, 'x', f, Hls, 'o', f1, H1, ':', f1, Hls1, '-');
axis([f(1), f(N), 0 ceil(10*max([H1, Hls1]))/10]);
xlabel('Frequency (Hz)')
ylabel('| H(f) |')
title('Frequency Response Magnitude')
legend('Measurement', 'LS Fit to Model', 'True (no noise)', 'LS Fit to Model')

% Calculate values from X, Y, and Z

w0 = (X/Z)^(.25);
B = sqrt((Y/Z) + 2*sqrt(X/Z));
Q = w0/B;

fprintf('Actual values:\n');
fprintf('w0 = %f Hz, B = %f Hz, Q = %f\n', 1/sqrt(L*C)/2/pi, (R+r)/L/2/pi, sqrt(L/C)/(R+r));

fprintf('Values computed from X, Y, Z using N = %d points:\n', N);
fprintf('w0 = %f Hz, B = %f Hz, Q = %f\n', w0/2/pi, B/2/pi, Q);



