% [sys, R] = sdpfilt (w, U, L, n)
% or
% [sys, R] = sdpfilt (w, U, L, n, Ts)
%
% This function tries to find an nth-order discrete-time filter defined
% by b(q^-1)/a(q^-1) such that its frequency response is between U(w)
% and L(w) The frequency is defined by the vector w goes from 0 to pi.
%
% First the power spectrum, R, is designed using LMIs. Secondly, the
% filter polynomials b and a are recovered using spectral factorization.
% 
% You can check the solution by
%
% plot (abs (frd (sys, w)), w, U, w, L, w, sqrt(R))
%
% If sdpfilt cannot achieve the bounds the LMI solver should give you
% some warnings. For instance, SeDuMI issues the warning
% "Run into numerical problems"
% or "Dual infeasible, primal improving direction found."
%
% © 2003, 2007, 2009 Anders Helmersson, ISY, Linköping
%
% This function was inspired by "FIR filter design via semidefinite
% programming and spectral factorization" by Shao-Po Wu, Stephen Boyd,
% and Lieven Vandenberghe. Proc. CDC, pp. 271-276, 1996.
%
% this function depends on Yalmip version 3, refer to
% http://www.control.isy.liu.se/~johanl/yalmip.html

function [sys, R, resp] = sdpfilt (w, U, L, n, Ts)

if nargin < 4,
  disp ('usage: [sys, R] = sdpfilt (w, U, L, n)');
  disp ('or');
  disp ('usage: [sys, R] = sdpfilt (w, U, L, n, Ts)');
  return;
end

if nargin == 5,
  % convert w from continuous to discrete time
  w = 2 * atan (0.5*Ts*w);
else
  Ts = 1;
end

A = [zeros(1,n); eye(n-1) zeros(n-1,1)];
B = [1; zeros(n-1,1)];

yalmip ('clear');

Ca = sdpvar (1, n);
Da = 1;                       % here we force ra(1) = 2;
ra = [2*Da Ca];
Pa = sdpvar (n, n, 'symmetric');

Cb = sdpvar (1, n);
Db = sdpvar (1, 1);
rb = [2*Db Cb];
Pb = sdpvar (n, n, 'symmetric');

% These two LMIs assure that we can factor both Ha and Hd
F = set ([Pa-A'*Pa*A  Ca'-A'*Pa*B; Ca-B'*Pa*A  Da+Da'-B'*Pa*B] > 0);
F = F + set ([Pb-A'*Pb*A  Cb'-A'*Pb*B; Cb-B'*Pb*A  Db+Db'-B'*Pb*B] > 0);

for i = 1 : length(w),
  ABw = (exp(j*w(i))*eye(n) - A) \ B;
  Ha = Da + Ca * ABw;
  HHa = Ha + Ha';
  Hb = Db + Cb * ABw;
  HHb = Hb + Hb';
  F = F + set (HHb > HHa*L(i)^2);
  F = F + set (HHb < HHa*U(i)^2);
end 

sol = solvesdp (F, []);

if sol.problem,
  disp ('Warning: the solution may not be reliable -- please check');
end

Ca = double (Ca);
Da = double (Da);
Cb = double (Cb);
Db = double (Db);
for i = 1 : length(w),
  ABw = (exp(j*w(i))*eye(n) - A) \ B;
  Ha = Da + Ca * ABw;
  Hb = Db + Cb * ABw;
  R(i) = (Hb + Hb') / (Ha + Ha');
end 

% spectral factorization
% resp =  genphase (vpck (sqrt (R(:)), w), 1);
% [b, a] = invfreqz (vunpck (resp), w, n, n);
resp = genphase (frd (sqrt(R), w, 1));
sys = fitfrd (resp, n);
sys.Ts = Ts;
if nargin == 5,
  % convert model from discrete to continuous time
  sys = d2c (sys, 'tustin');
end
