filtr pasmowoprzepustowy metodą aproksymacji czebyszewa (algorytm remeza)

%Napisz program do projektowania metodą aproksymacji Czebyszewa (algorytm Remeza)
%filtr pasmowoprzepustowy. Wykreśl: odpowiedź impulsową,
%charakterystykę amplitudową i fazową filtra. 
%W projekcie umieść informację o podstawach teoretycznych rozważanego zagadnienia

L = 20;             % liczba poszukiwanych wspolczynnikow filtra (parzysta): dlugosc filtra N=2L-1
  Nr = 5;             % szerokosc pasma przepustowego 0 < Nr < L
  wp = 1; ws = 1;      % wagi poszczególnych podpasm: Pass, Stop, mozna zmieniac, przez co usyskuje sie bardziej gladka charakterystykę
  R = 200;            % ile razy zbiór testowy ma byl wiekszy od zbioru ekstremów
  tol = 10^(-8);     % tolerancja rozwiazania
 
  M = L+1;            % liczba czestotliwosci ekstremów
  K = 1+R*(M-1);      % liczba wszystkich badanych czestotliwosci
  fz = (0 : K-1)/(K-1);                   % K-elementowy zbiór czestotliwosci przeszukiwanych
  k11 = 1; k12 = 1+Nr*R;                  % granice pasma przepustowego
  k21 = 1+(Nr+1)*R; k22=K;                % granice pasma zaporowego
  K1 = 1+Nr*R+R/2;                        % nr próbki charakterystyki dla czest. granicznej
  fd = [ fz(1:K1) fz(K1:K)];              % czestotliwosci charakterystyczne filtra
  Hd = [ ones(1,K1) zeros(1,K-K1)];       % wymagane wartosci wzmocnienia
  Wg = [ wp*ones(1,K1) ws*ones(1,K-K1)];  % wagi
  imax = 1:R:K;                           % indeksy startowych czestotliwosci ekstremów
% Wybranie startowego zbioru czestotliwosci "ekstremów"
  feMAX = fz(1:R:K);
  sigmaMAX = 10^15;
  sigma = 0;
 
% Pętla główna
  n = 0 : L-1;
  while ( (sigmaMAX-sigma) > tol )
 
       sigmaMAX - sigma     % pokaz aktualne zmniejszenie wartosci bledu ch-ki
        H = Hd(imax); H=H';
        W = Wg(imax);
      % Uaktualnij parametry zmieniane iteracyjnie
        fe = feMAX;          % nowe czestotliwosci ekstremów w obecnym kroku
      % Oblicz macierz kosinusów A
        A = [];
        for m = 0 : M-1      % po czestotliwosciach ekstremów
            A = [ A;  cos(pi*fe(m+1)*n) ((-1)^m)/W(m+1) ];
        end
      % Rozwiąż równanie
     
        c = pinv(A)*H;       %pinv- inwersja macierzy
      %   aktualna odpowiedz impulsowa
        h = c(1:L); sigma=c(M); sigma=abs(sigma);
        g=h'/2; g(1)=2*g(1); g = [ fliplr(g(2:L)) g];
       
      % Oblicz aktualna charakterystyke czestotliwosciowa oraz jej blad
        for k = 0 : K-1
          H(k+1) = cos(pi*fz(k+1)*n) * h;
          Herr(k+1) = Wg(k+1) * (H(k+1) - Hd(k+1));
         end
           
      % Znajdz M+1 najwiekszych ekstremów funkcji bledu (czestotliwosci, wartosci ekstremum)
      % tzn. oblicz feMAX i sigmaMAX
     
      % Znajdź wszystkie ekstrema
        Hmax = []; imax = [];
        for p = 1 : 2                     % kolejne przedzialy: przepuszczania i zaporowy
           if (p==1) k1=k11; k2=k12; end  % pasmo przepustowe
           if (p==2) k1=k21; k2=k22; end  % pasmo zaporowe
           Hmax = [ Hmax Herr(k1)]; imax = [ imax k1 ];  % zapisz pierwszy element / indeks
           k=k1+1;                                       % zwieksz indeks
           while( Herr(k-1) == Herr(k) ) k = k+1; end    % inkrementuj indeks, jezli równe
           if ( Herr(k) < Herr(k+1) )     %
             sgn=1;                       % charakterystyka narasta
           else                           %
             sgn=-1;                      % charakterystyka opada
           end                            %
           k=k+1;                         %
           while ( k <= k2 )              % ?poluj? na kolejne ekstrema
             if (sgn==1)
                while( (k<k2) & (Herr(k-1)<Herr(k)) ) k=k+1; end
             end
             if (sgn==-1)
                while( (k<k2) & (Herr(k-1)>Herr(k)) ) k=k+1; end
             end
             sgn = -sgn;
             Hmax = [ Hmax Herr(k) ]; imax = [imax k];  % zapamiętaj kolejne ekstremum
             k=k+1;
           end % end while
        end % end for
       
      % Wybierz M+1 najwiekszych
        if ( length(Hmax)>M ) 
           disp('UWAGA!!! Wiecej EKSTREMÓW niż M+1!');
           IM = []; G = abs(Hmax); LenG = length(G);
           while( LenG > 0 )
             Gmx = max(G); imx=find(G==Gmx);
             LenGmx = length(imx);
             IM = [ IM imax(imx)];
             G(imx)=0; LenG = LenG-LenGmx;
           end
           IM = IM(1:M); IM = sort(IM); imax = IM;
        end
        sigmaMAX = max( abs(Hmax) );
        feMAX = fz( imax );
       
  end % end while(tol)
 
% -------------WYKRESY-----------------
  fz=fz/2;
  figure(1);stem(g); title('Wynikowa odp impulsowa filtra');
  figure(2);plot(fz(imax),Herr(imax),'or',fz,Herr,'b'); grid;
  figure(3);plot(fz,Hd,'r',fz,H,'b'); grid; title('Wynikowe H(f)');
  figure(4);plot(fz,20*log10(H),'b'); grid; title('Wynikowe H(f) w dB'); pause