parksmcclellan.cpp

Go to the documentation of this file.

//=============================================================================================================
 
// Adapted from the C port of the Parks-McClellan Fortran program by Iowa Hills Software
// (http://www.iowahills.com/A7ExampleCodePage.html, October 19, 2013); ported to MNE-CPP by
// Christoph Dinh and Florian Schlembach in February 2014.
 
//=============================================================================================================
// INCLUDES
//=============================================================================================================
 
#include "parksmcclellan.h"
 
//=============================================================================================================
// QT INCLUDES
//=============================================================================================================
 
#include <qmath.h>
 
//=============================================================================================================
// USED NAMESPACES
//=============================================================================================================
 
using namespace UTILSLIB;
using namespace Eigen;
 
//=============================================================================================================
// DEFINES
//=============================================================================================================
 
constexpr int BIG = 4096; // Used to define array sizes. Must be somewhat larger than 8 * MaxNumTaps
constexpr int SMALL = 256;
constexpr double M_2PI = 6.28318530717958647692;
constexpr int ITRMAX = 50;              // Max Number of Iterations. Some filters require as many as 45 iterations.
constexpr double MIN_TEST_VAL = 1.0E-6; // Min value used in LeGrangeInterp and GEE
 
//=============================================================================================================
// DEFINE MEMBER METHODS
//=============================================================================================================
 
ParksMcClellan::ParksMcClellan()
: HalfTapCount(0)
, ExchangeIndex(SMALL)
, LeGrangeD(SMALL)
, Alpha(SMALL)
, CosOfGrid(SMALL)
, DesPlus(SMALL)
, Coeff(SMALL)
, Edge(SMALL)
, BandMag(SMALL)
, InitWeight(SMALL)
, DesiredMag(BIG)
, Grid(BIG)
, Weight(BIG)
, InitDone2(false)
{
}
 
//=============================================================================================================
 
ParksMcClellan::ParksMcClellan(int NumTaps, double OmegaC, double BW, double ParksWidth, TPassType PassType)
: HalfTapCount(0)
, ExchangeIndex(SMALL)
, LeGrangeD(SMALL)
, Alpha(SMALL)
, CosOfGrid(SMALL)
, DesPlus(SMALL)
, Coeff(SMALL)
, Edge(SMALL)
, BandMag(SMALL)
, InitWeight(SMALL)
, DesiredMag(BIG)
, Grid(BIG)
, Weight(BIG)
, InitDone2(false)
{
    init(NumTaps, OmegaC, BW, ParksWidth, PassType);
}
 
//=============================================================================================================
 
ParksMcClellan::~ParksMcClellan()
{
}
 
//=============================================================================================================
 
void ParksMcClellan::init(int NumTaps, double OmegaC, double BW, double ParksWidth, TPassType PassType)
{
    // Set per pass type below; an unrecognised type leaves the band table empty
    // rather than reading an indeterminate count.
    int j, NumBands = 0;
 
    if (NumTaps > 256)
        NumTaps = 256;
    if (NumTaps < 9)
        NumTaps = 9;
    if ((PassType == HPF || PassType == NOTCH) && NumTaps % 2 == 0)
        NumTaps--;
 
    // It helps the algorithm a great deal if each band is at least 0.01 wide.
    // The weights used here came from the orig PM code.
    if (PassType == LPF) {
        NumBands = 2;
        Edge[1] = 0.0;    // Omega = 0
        Edge[2] = OmegaC; // Pass band edge
        if (Edge[2] < 0.01)
            Edge[2] = 0.01;
        if (Edge[2] > 0.98)
            Edge[2] = 0.98;
        Edge[3] = Edge[2] + ParksWidth; // Stop band edge
        if (Edge[3] > 0.99)
            Edge[3] = 0.99;
        Edge[4] = 1.0; // Omega = Pi
 
        BandMag[1] = 1.0;
        BandMag[2] = 0.0;
        InitWeight[1] = 1.0;
        InitWeight[2] = 10.0;
    }
 
    if (PassType == HPF) {
        NumBands = 2;
        Edge[1] = 0.0;    // Omega = 0
        Edge[3] = OmegaC; // Pass band edge
        if (Edge[3] > 0.99)
            Edge[3] = 0.99;
        if (Edge[3] < 0.02)
            Edge[3] = 0.02;
        Edge[2] = Edge[3] - ParksWidth; // Stop band edge
        if (Edge[2] < 0.01)
            Edge[2] = 0.01;
        Edge[4] = 1.0; // Omega = Pi
 
        BandMag[1] = 0.0;
        BandMag[2] = 1.0;
        InitWeight[1] = 10.0;
        InitWeight[2] = 1.0;
    }
 
    if (PassType == BPF) {
        NumBands = 3;
        Edge[1] = 0.0;               // Omega = 0
        Edge[3] = OmegaC - BW / 2.0; // Left pass band edge.
        if (Edge[3] < 0.02)
            Edge[3] = 0.02;
        Edge[2] = Edge[3] - ParksWidth; // Left stop band edge
        if (Edge[2] < 0.01)
            Edge[2] = 0.01;
        Edge[4] = OmegaC + BW / 2.0; // Right pass band edge
        if (Edge[4] > 0.98)
            Edge[4] = 0.98;
        Edge[5] = Edge[4] + ParksWidth; // Right stop band edge
        if (Edge[5] > 0.99)
            Edge[5] = 0.99;
        Edge[6] = 1.0; // Omega = Pi
 
        BandMag[1] = 0.0;
        BandMag[2] = 1.0;
        BandMag[3] = 0.0;
        InitWeight[1] = 10.0;
        InitWeight[2] = 1.0;
        InitWeight[3] = 10.0;
    }
 
    if (PassType == NOTCH) {
        NumBands = 3;
        Edge[1] = 0.0;               // Omega = 0
        Edge[3] = OmegaC - BW / 2.0; // Left stop band edge.
        if (Edge[3] < 0.02)
            Edge[3] = 0.02;
        Edge[2] = Edge[3] - ParksWidth; // Left pass band edge
        if (Edge[2] < 0.01)
            Edge[2] = 0.01;
        Edge[4] = OmegaC + BW / 2.0; // Right stop band edge
        if (Edge[4] > 0.98)
            Edge[4] = 0.98;
        Edge[5] = Edge[4] + ParksWidth; // Right pass band edge
        if (Edge[5] > 0.99)
            Edge[5] = 0.99;
        Edge[6] = 1.0; // Omega = Pi
 
        BandMag[1] = 1.0;
        BandMag[2] = 0.0;
        BandMag[3] = 1.0;
        InitWeight[1] = 1.0;
        InitWeight[2] = 10.0;
        InitWeight[3] = 1.0;
    }
 
    // Parks McClellan's edges are based on 2Pi, we are based on Pi.
    for (j = 1; j <= 2 * NumBands; j++)
        Edge[j] /= 2.0;
 
    CalcParkCoeff2(NumBands, NumTaps);
}
 
//=============================================================================================================
 
void ParksMcClellan::CalcParkCoeff2(int NumBands, int TapCount)
{
    int j, k, GridCount, GridIndex, BandIndex, NumIterations;
    double LowFreqEdge, UpperFreq, TempVar, Change;
    bool OddNumTaps;
    GridCount = 16; // Grid Density
 
    if (TapCount % 2)
        OddNumTaps = true;
    else
        OddNumTaps = false;
 
    HalfTapCount = TapCount / 2;
    if (OddNumTaps)
        HalfTapCount++;
 
    Grid[1] = Edge[1];
    LowFreqEdge = GridCount * HalfTapCount;
    LowFreqEdge = 0.5 / LowFreqEdge;
    j = 1;
    k = 1;
    BandIndex = 1;
    while (BandIndex <= NumBands) {
        UpperFreq = Edge[k + 1];
        while (Grid[j] <= UpperFreq) {
            TempVar = Grid[j];
            DesiredMag[j] = BandMag[BandIndex];
            Weight[j] = InitWeight[BandIndex];
            j++;
            ;
            Grid[j] = TempVar + LowFreqEdge;
        }
 
        Grid[j - 1] = UpperFreq;
        DesiredMag[j - 1] = BandMag[BandIndex];
        Weight[j - 1] = InitWeight[BandIndex];
        k += 2;
        BandIndex++;
        if (BandIndex <= NumBands)
            Grid[j] = Edge[k];
    }
 
    GridIndex = j - 1;
    if (!OddNumTaps && Grid[GridIndex] > (0.5 - LowFreqEdge))
        GridIndex--;
 
    if (!OddNumTaps) {
        for (j = 1; j <= GridIndex; j++) {
            Change = cos(M_PI * Grid[j]);
            DesiredMag[j] = DesiredMag[j] / Change;
            Weight[j] = Weight[j] * Change;
        }
    }
 
    TempVar = static_cast<double>(GridIndex - 1) / static_cast<double>(HalfTapCount);
    for (j = 1; j <= HalfTapCount; j++) {
        ExchangeIndex[j] = static_cast<double>(j - 1) * TempVar + 1.0;
    }
    ExchangeIndex[HalfTapCount + 1] = GridIndex;
 
    NumIterations = Remez2(GridIndex);
    CalcCoefficients();
 
    // Calculate the impulse response.
    if (OddNumTaps) {
        for (j = 1; j <= HalfTapCount - 1; j++) {
            Coeff[j] = 0.5 * Alpha[HalfTapCount + 1 - j];
        }
        Coeff[HalfTapCount] = Alpha[1];
    } else {
        Coeff[1] = 0.25 * Alpha[HalfTapCount];
        for (j = 2; j <= HalfTapCount - 1; j++) {
            Coeff[j] = 0.25 * (Alpha[HalfTapCount + 1 - j] + Alpha[HalfTapCount + 2 - j]);
        }
        Coeff[HalfTapCount] = 0.5 * Alpha[1] + 0.25 * Alpha[2];
    }
 
    // Output section: TapCount may differ from the count the object was constructed with (clamped, or init() later)
    FirCoeff = RowVectorXd::Zero(TapCount);
    for (j = 1; j <= HalfTapCount; j++)
        FirCoeff[j - 1] = Coeff[j];
    if (OddNumTaps)
        for (j = 1; j < HalfTapCount; j++)
            FirCoeff[HalfTapCount + j - 1] = Coeff[HalfTapCount - j];
    else
        for (j = 1; j <= HalfTapCount; j++)
            FirCoeff[HalfTapCount + j - 1] = Coeff[HalfTapCount - j + 1];
 
    // Parks2Label was on my application's main form.
    // These replace the original Ouch() function
    if (NumIterations <= 3) {
        //TopForm->Parks2Label->Font->Color = clRed;
        //TopForm->Parks2Label->Caption = "Convergence ?";  // Covergence is doubtful, but possible.
    } else {
        //TopForm->Parks2Label->Font->Color = clBlack;
        //TopForm->Parks2Label->Caption = UnicodeString(NumIterations) + " Iterations";
    }
}
 
//=============================================================================================================
 
int ParksMcClellan::Remez2(int GridIndex)
{
    // This routine is a direct port of the original goto-driven Fortran, so the
    // compiler cannot prove that every path assigns these before use. Give them
    // defined starting values rather than relying on the control flow.
    int j, JET, K, k, NU, JCHNGE, K1, KNZ, KLOW, NUT, KUP;
    int NUT1 = 0, LUCK, KN, NITER;
    double Deviation, DNUM, DDEN, TempVar;
    double DEVL, COMP = 0.0, YNZ = 0.0, Y1, ERR;
 
    Y1 = 1;
    LUCK = 0;
    DEVL = -1.0;
    NITER = 1; // Init this to 1 to be consistent with the orig code.
 
TOP_LINE: // We come back to here from 3 places at the bottom.
    ExchangeIndex[HalfTapCount + 2] = GridIndex + 1;
 
    for (j = 1; j <= HalfTapCount + 1; j++) {
        TempVar = Grid[ExchangeIndex[j]];
        CosOfGrid[j] = cos(TempVar * M_2PI);
    }
 
    JET = (HalfTapCount - 1) / 15 + 1;
    for (j = 1; j <= HalfTapCount + 1; j++) {
        LeGrangeD[j] = LeGrangeInterp2(j, HalfTapCount + 1, JET);
    }
 
    DNUM = 0.0;
    DDEN = 0.0;
    K = 1;
    for (j = 1; j <= HalfTapCount + 1; j++) {
        k = ExchangeIndex[j];
        DNUM += LeGrangeD[j] * DesiredMag[k];
        DDEN += static_cast<double>(K) * LeGrangeD[j] / Weight[k];
        K = -K;
    }
    Deviation = DNUM / DDEN;
 
    NU = 1;
    if (Deviation > 0.0)
        NU = -1;
    Deviation = -static_cast<double>(NU) * Deviation;
    K = NU;
    for (j = 1; j <= HalfTapCount + 1; j++) {
        k = ExchangeIndex[j];
        TempVar = static_cast<double>(K) * Deviation / Weight[k];
        DesPlus[j] = DesiredMag[k] + TempVar;
        K = -K;
    }
 
    if (Deviation <= DEVL)
        return (NITER); // Ouch
 
    DEVL = Deviation;
    JCHNGE = 0;
    K1 = ExchangeIndex[1];
    KNZ = ExchangeIndex[HalfTapCount + 1];
    KLOW = 0;
    NUT = -NU;
 
    //Search for the extremal frequencies of the best approximation.
 
    j = 1;
    while (j < HalfTapCount + 2) {
        KUP = ExchangeIndex[j + 1];
        k = ExchangeIndex[j] + 1;
        NUT = -NUT;
        if (j == 2)
            Y1 = COMP;
        COMP = Deviation;
 
        if (k < KUP && !ErrTest(k, NUT, COMP, &ERR)) {
        L210:
            COMP = static_cast<double>(NUT) * ERR;
            for (k++; k < KUP; k++) {
                if (ErrTest(k, NUT, COMP, &ERR))
                    break; // for loop
                COMP = static_cast<double>(NUT) * ERR;
            }
 
            ExchangeIndex[j] = k - 1;
            j++;
            KLOW = k - 1;
            JCHNGE++;
            continue; // while loop
        }
 
        k--;
 
    L225:
        k--;
        if (k <= KLOW) {
            k = ExchangeIndex[j] + 1;
            if (JCHNGE > 0) {
                ExchangeIndex[j] = k - 1;
                j++;
                KLOW = k - 1;
                JCHNGE++;
                continue; // while loop
            } else        // JCHNGE <= 0
            {
                for (k++; k < KUP; k++) {
                    if (ErrTest(k, NUT, COMP, &ERR))
                        continue; // for loop
                    goto L210;
                }
 
                KLOW = ExchangeIndex[j];
                j++;
                continue; // while loop
            }
        }
        // Can't use a do while loop here, it would outdent the two continue statements.
        if (ErrTest(k, NUT, COMP, &ERR) && JCHNGE <= 0)
            goto L225;
 
        if (ErrTest(k, NUT, COMP, &ERR)) {
            KLOW = ExchangeIndex[j];
            j++;
            continue; // while loop
        }
 
        COMP = static_cast<double>(NUT) * ERR;
 
    L235:
        for (k--; k > KLOW; k--) {
            if (ErrTest(k, NUT, COMP, &ERR))
                break; // for loop
            COMP = static_cast<double>(NUT) * ERR;
        }
 
        KLOW = ExchangeIndex[j];
        ExchangeIndex[j] = k + 1;
        j++;
        JCHNGE++;
    } // end while(j<HalfTapCount
 
    if (j == HalfTapCount + 2)
        YNZ = COMP;
 
    while (j <= HalfTapCount + 2) {
        if (K1 > ExchangeIndex[1])
            K1 = ExchangeIndex[1];
        if (KNZ < ExchangeIndex[HalfTapCount + 1])
            KNZ = ExchangeIndex[HalfTapCount + 1];
        NUT1 = NUT;
        NUT = -NU;
        k = 0;
        KUP = K1;
        COMP = YNZ * 1.00001;
        LUCK = 1;
 
        for (k++; k < KUP; k++) {
            if (ErrTest(k, NUT, COMP, &ERR))
                continue; // for loop
            j = HalfTapCount + 2;
            goto L210;
        }
        LUCK = 2;
        break; // break while(j <= HalfTapCount+2) loop
    } // end while(j <= HalfTapCount+2)
 
    if (LUCK == 1 || LUCK == 2) {
        if (LUCK == 1) {
            if (COMP > Y1)
                Y1 = COMP;
            K1 = ExchangeIndex[HalfTapCount + 2];
        }
 
        k = GridIndex + 1;
        KLOW = KNZ;
        NUT = -NUT1;
        COMP = Y1 * 1.00001;
 
        for (k--; k > KLOW; k--) {
            if (ErrTest(k, NUT, COMP, &ERR))
                continue; // for loop
            j = HalfTapCount + 2;
            COMP = static_cast<double>(NUT) * ERR;
            LUCK = 3; // last time in this if(LUCK == 1 || LUCK == 2)
            goto L235;
        }
 
        if (LUCK == 2) {
            if (JCHNGE > 0 && NITER++ < ITRMAX)
                goto TOP_LINE;
            else
                return (NITER);
        }
 
        for (j = 1; j <= HalfTapCount; j++) {
            ExchangeIndex[HalfTapCount + 2 - j] = ExchangeIndex[HalfTapCount + 1 - j];
        }
        ExchangeIndex[1] = K1;
        if (NITER++ < ITRMAX)
            goto TOP_LINE;
    } // end if(LUCK == 1 || LUCK == 2)
 
    KN = ExchangeIndex[HalfTapCount + 2];
    for (j = 1; j <= HalfTapCount; j++) {
        ExchangeIndex[j] = ExchangeIndex[j + 1];
    }
    ExchangeIndex[HalfTapCount + 1] = KN;
    if (NITER++ < ITRMAX)
        goto TOP_LINE;
 
    return (NITER);
}
 
//=============================================================================================================
 
double ParksMcClellan::LeGrangeInterp2(int K, int N, int M) // D
{
    int j, k;
    double Dee, Q;
    Dee = 1.0;
    Q = CosOfGrid[K];
    for (k = 1; k <= M; k++)
        for (j = k; j <= N; j += M) {
            if (j != K)
                Dee = 2.0 * Dee * (Q - CosOfGrid[j]);
        }
    if (std::fabs(Dee) < MIN_TEST_VAL) {
        if (Dee < 0.0)
            Dee = -MIN_TEST_VAL;
        else
            Dee = MIN_TEST_VAL;
    }
    return (1.0 / Dee);
}
 
//=============================================================================================================
 
double ParksMcClellan::GEE2(int K, int N)
{
    int j;
    double P, C, Dee, XF;
    P = 0.0;
    XF = Grid[K];
    XF = cos(M_2PI * XF);
    Dee = 0.0;
    for (j = 1; j <= N; j++) {
        C = XF - CosOfGrid[j];
        if (std::fabs(C) < MIN_TEST_VAL) {
            if (C < 0.0)
                C = -MIN_TEST_VAL;
            else
                C = MIN_TEST_VAL;
        }
        C = LeGrangeD[j] / C;
        Dee = Dee + C;
        P = P + C * DesPlus[j];
    }
    if (std::fabs(Dee) < MIN_TEST_VAL) {
        if (Dee < 0.0)
            Dee = -MIN_TEST_VAL;
        else
            Dee = MIN_TEST_VAL;
    }
    return (P / Dee);
}
 
//=============================================================================================================
 
bool ParksMcClellan::ErrTest(int k, int Nut, double Comp, double* Err)
{
    *Err = GEE2(k, HalfTapCount + 1);
    *Err = (*Err - DesiredMag[k]) * Weight[k];
    if (static_cast<double>(Nut) * *Err - Comp <= 0.0)
        return (true);
    else
        return (false);
}
 
//=============================================================================================================
 
void ParksMcClellan::CalcCoefficients()
{
    int j, k, n;
    double GTempVar, OneOverNumTaps;
    double Omega, TempVar, FreqN, TempX, GridCos;
    double GeeArray[SMALL];
 
    GTempVar = Grid[1];
    CosOfGrid[HalfTapCount + 2] = -2.0;
    OneOverNumTaps = 1.0 / static_cast<double>(2 * HalfTapCount - 1);
    k = 1;
 
    for (j = 1; j <= HalfTapCount; j++) {
        FreqN = static_cast<double>(j - 1) * OneOverNumTaps;
        TempX = cos(M_2PI * FreqN);
 
        GridCos = CosOfGrid[k];
        if (TempX <= GridCos) {
            while (TempX <= GridCos && (GridCos - TempX) >= MIN_TEST_VAL) // MIN_TEST_VAL = 1.0E-6
            {
                k++;
                ;
                GridCos = CosOfGrid[k];
            }
        }
        if (TempX <= GridCos || (TempX - GridCos) < MIN_TEST_VAL) {
            GeeArray[j] = DesPlus[k]; // Desired Response
        } else {
            Grid[1] = FreqN;
            GeeArray[j] = GEE2(1, HalfTapCount + 1);
        }
        if (k > 1)
            k--;
    }
 
    Grid[1] = GTempVar;
    for (j = 1; j <= HalfTapCount; j++) {
        TempVar = 0.0;
        Omega = static_cast<double>(j - 1) * M_2PI * OneOverNumTaps;
        for (n = 1; n <= HalfTapCount - 1; n++) {
            TempVar += GeeArray[n + 1] * cos(Omega * static_cast<double>(n));
        }
        TempVar = 2.0 * TempVar + GeeArray[1];
        Alpha[j] = TempVar;
    }
 
    Alpha[1] = Alpha[1] * OneOverNumTaps;
    for (j = 2; j <= HalfTapCount; j++) {
        Alpha[j] = 2.0 * Alpha[j] * OneOverNumTaps;
    }
}