Implementation of Optimum Filter

QOptimumFilter.cc

#include "QOptimumFilter.hh"
#include "QError.hh"
#include "QMatrix.hh"
#include "QRealComplexFFT.hh"
#include "QFir.hh"
#include <sstream>

// EXTRA_JITTERS:
// 1: minimum number needed for interpolation
// 2: more point to see if we have a minimum at the boundary (enable nice parabola)

using std::vector;
using namespace Cuore;

QOptimumFilter::QOptimumFilter(const QVector& ap1, const QVector& an, int maxJitter, bool useDiff, bool debugOn) : 
    fMaxJitter(maxJitter),
    fDebugOn(debugOn),
    fUseDiff(useDiff),
    fTransformer(0),
    fSize(0),
    fCutOffFrequency(Q_DOUBLE_DEFAULT)
{
    fCheckForFilteredSamples = QERR_UNKNOWN_ERR;
    fCheckForFilteredSamples.SetDescription(__FILE__,__LINE__,"Filtered samples not available");
    fSize = ap1.Size();
    QError err;
    if(an.Size() != fSize) {
        err = QERR_SIZE_NOT_MATCH;
        err.SetDescription(__FILE__,__LINE__,"Size of input QVectors mismatches");
        CuoreThrow(err);
    }

    fAveragePulse1 = NormalizeVector(ap1);
    fMaxPos = fAveragePulse1.GetMaxIndex();
    fAvgPulseIntegral1= fAveragePulse1.Sum(fAveragePulse1.Size(),0);

    // compute the time span from half-rise to maximum
    fAvgPulseHalfTimeToMax = -1;
    double A = fAveragePulse1.GetMax()-fAveragePulse1[0];
    for(int i = fMaxPos; i > 0; i--) {
        if( (fAveragePulse1[i]-fAveragePulse1[0])<0.5*A ) { 
            fAvgPulseHalfTimeToMax = (fMaxPos-i);
            break;
        }
    }
    if(fAvgPulseHalfTimeToMax < 1 || fAvgPulseHalfTimeToMax >= fMaxPos) {
        err = QERR_OUT_OF_RANGE;
        err.SetDescription(__FILE__,__LINE__,"failed to autoset the maximum jitter");
        CuoreThrow(err);
    }
    // autoset maximum jitter

    if(fMaxJitter < 0) fMaxJitter = fAvgPulseHalfTimeToMax*2; 

    fTransformer = new QRealComplexFFT(fSize);
    QVectorC apf; fTransformer->TransformToFreq(fAveragePulse1,apf);
    fAveragePulseEnergySpectrum =  apf.GetMagnitudesSquare();

    fAverageNoise = an;
    fAverageNoise[0] = 0;

    err = BuildFilter();
    if(err != QERR_SUCCESS) CuoreThrow(err);
}

QOptimumFilter::~QOptimumFilter()
{
    if(fTransformer) {
        delete fTransformer;
        fTransformer = 0;
    }
}

QError QOptimumFilter::Filter(const Cuore::QVector& p)
{
    fCheckForFilteredSamples = QERR_SUCCESS;
    fFiltered1.Clear(); 
    if(p.Size() != fSize) {
        QError err(QERR_SIZE_NOT_MATCH,__FILE__,__LINE__,"Size of input QVector is not correct");
        fCheckForFilteredSamples = err;
        return fCheckForFilteredSamples;
    }
    if(fUseDiff) { 
        Cuore::QVector diff = p;
        diff.Differentiate();
        fTransformer->TransformToFreq(diff,fTransformedWaveform);
    } else {
        fTransformer->TransformToFreq(p,fTransformedWaveform);
    }
    fTransformedWaveform[0].Set(0,0);
    fTransformer->TransformFromFreq(fFilter1.Mult(fTransformedWaveform),fFiltered1);

    return fCheckForFilteredSamples;
}

const Cuore::QVector&  QOptimumFilter::GetFiltered() const 
{ 
    if(fCheckForFilteredSamples != QERR_SUCCESS) CuoreThrow(fCheckForFilteredSamples);
    return fFiltered1; 
}

Cuore::QVector  QOptimumFilter::GetFilteredShifted() const 
{ 
    QVector filtShift = GetFiltered();
    filtShift.Shift(fMaxPos);
    return filtShift;
}


QError QOptimumFilter::GetInterpolated(double& intJitter, double& chi2,double& amplitude, double& integral) const
{
    double dummy, yb,chib;
    QError err = Get(dummy, chib, yb, dummy);
    if(err != QERR_SUCCESS) return err;

    double xa = fOptimumJitter-1;
    double xb = fOptimumJitter;
    double xc = fOptimumJitter+1; 

    int ia = fOptimumJitter-1; if(ia < 0) ia+=fSize;
    double ya = fFiltered1[ia];

    int ic = fOptimumJitter+1; if(ic >= (int)fSize) ic-=fSize;
    double yc = fFiltered1[ic];

    if(yb<ya || yb<yc) {
        QError err = QERR_OUT_OF_RANGE;
        std::stringstream msg;
        msg<<"Amplitude "<<yb<<" not at maximum. Boundaries: ["<<ya<<" "<<yc<<"]";
        err.SetDescription(__FILE__,__LINE__,msg.str());  
        return err;
    }

    double detA = xa*xa*(xb-xc)-xb*xb*(xa-xc)+xc*xc*(xa-xb);
    double detA1 = ya*(xb-xc)-yb*(xa-xc)+yc*(xa-xb);
    double detA2 = xa*xa*(yb-yc)-xb*xb*(ya-yc)+xc*xc*(ya-yb);
    double detA3 = xa*xa*(xb*yc-yb*xc)-xb*xb*(xa*yc-ya*xc)+xc*xc*(xa*yb-ya*xb);

    double p0 = detA1/detA;
    double p1 = detA2/detA;
    double p2 = detA3/detA;

    amplitude = -p1*p1/(4.*p0)+p2;  intJitter = -p1/2./p0;
    integral = fAvgPulseIntegral1*amplitude;

    intJitter = -p1/2./p0;
    if(intJitter <xa ||  intJitter >xc) {
        QError err = QERR_OUT_OF_RANGE;
        std::stringstream msg;
        msg<<"Interpolated jitter "<<intJitter<<" out of boundaries ["<<xa<<" "<<xc<<"]";
        err.SetDescription(__FILE__,__LINE__,msg.str());  
        return err;
    }

    // interpolate chi2
    int low = floor(intJitter); 
    double chi1Low, chi1High;
    if(low == xa) {
        const Cuore::QVectorC& ap1fa = fAveragePulse1Shifted[ia];
        double chia = (fTransformedWaveform-ya*ap1fa).GetModulusSquare().Div(fAverageNoiseForChi2).Sum(fSize-1,1);
        chia /= (fSize-2);
        chi1Low = chia;
        chi1High = chib;
    } else {
        const Cuore::QVectorC& ap1fc = fAveragePulse1Shifted[ic];
        double chic = (fTransformedWaveform-yc*ap1fc).GetModulusSquare().Div(fAverageNoiseForChi2).Sum(fSize-1,1);
        chic /= (fSize-2);
        chi1Low = chib;
        chi1High = chic;
    }

    double x =  intJitter-low;
    chi2 = (chi1High-chi1Low)*x+chi1Low;

    if(intJitter > (int)fSize/2) intJitter-=fSize; 
    return QERR_SUCCESS;
}

QVector QOptimumFilter::NormalizeVector(const QVector& vec) const
{
    QVector ret = vec;
    QVector baseline(ret.Size());
    baseline.Initialize(ret[0]);
    ret -= baseline;
    ret /= ret.GetMax();
    return ret;
}

QError QOptimumFilter::BuildFilter()
{
    QError err = QERR_SUCCESS;
    fDebugVectors.clear();
    fDebugSpectra.clear();

    QVector averageNoise = fAverageNoise;
    QVector averagePulse = fAveragePulse1;

    if(fUseDiff) {
        // differentiate noise and average pulse
        double pi = M_PI;
        for(size_t i = 0; i < fSize; i++){
            if(i <= averageNoise.Size()/2) averageNoise[i] *= 2.*(1-cos(2.*pi/(averageNoise.Size())*i));
            else averageNoise[i] *= 2.*(1-cos(2.*pi/(averageNoise.Size())*(averageNoise.Size()-i)));
        }
        averagePulse.Differentiate(); 
    }

    // force to zero the average pulse at the boundaries
    fEffectiveFilterLength = fSize-fMaxJitter*2;
    QVector window = QFFT::GetWindow(QFFT::WT_Cosinus,fEffectiveFilterLength,0,false,fMaxJitter);
    window = QFFT::ZeroPad(window,fSize-fEffectiveFilterLength, QFFT::kSymmetric,0);
    QVector ap = averagePulse.Mult(window);
    QVectorC ap1f;
    if(fDebugOn) {
        fTransformer->TransformToFreq(averagePulse,ap1f);
        fDebugSpectra.push_back(averageNoise); // 0  input average noise
        fDebugSpectra.push_back(ap1f.GetMagnitudesSquare()); // 1   average pulse 
        fDebugVectors.push_back(ap);  // 0 windowed average pulse
    }

    // create filter in FD
    QVectorC anc(fSize);
    anc.Initialize(0,0);
    anc.SetRe(averageNoise);
    fTransformer->TransformToFreq(ap,ap1f);
    QVector SNR = ap1f.GetModulusSquare().Div(averageNoise);
    double M = 1./SNR.Sum(fSize-1,1);
    fFilter1 = M*ap1f.Conj().Div(anc);
    fFilter1 *= fSize;
    fFilter1[0].Set(0,0);

    // get kernel in TD
    QVector kernel;
    fTransformer->TransformFromFreq(fFilter1,kernel);
    if(fDebugOn) {
        fDebugSpectra.push_back(ap1f.GetMagnitudesSquare()); // 2  windowed average pulse 
        fDebugSpectra.push_back(fFilter1.GetMagnitudesSquare()); // 3 filter in FD with windowed average pulse
        fDebugVectors.push_back(kernel); // 1 filter in TD with windowed average pulse
    }


    // determine SNR as a function of frequency
    fSNRFreq.Resize(fSize/2+1); fSNRFreq[0] = 0;
    for(size_t i = 1; i < fSize/2; i++) {
        fSNRFreq[i] = fSNRFreq[i-1]+2*SNR[i];
    }
    fSNRFreq[fSize/2] = fSNRFreq[fSize/2-1]+SNR[fSize/2];
    if(fDebugOn) {
        QVector filteredAverageNoise = fFilter1.GetModulusSquare().Mult(averageNoise); 
        fDebugSpectra.push_back(filteredAverageNoise);  // 4 filtered average noise with windowed average pulse
    }
    // filter out frequencies where SNR improvement is irrelevant   
    size_t cutOffFrequency = fSize/2;
    for( ; cutOffFrequency > 1; cutOffFrequency--) {
        if(fSNRFreq[cutOffFrequency] < 0.990*fSNRFreq[fSize/2]) break;
    }
    fCutOffFrequency = double(cutOffFrequency)/fSize; // normalize to Nyquist
    QCFirData firData;
    firData.fMethod = QCFirData::KayserBessel;
    firData.fCutoff= fCutOffFrequency;
    firData.fM= fMaxJitter*2;
    if(firData.fM%2 == 0) firData.fM--;
    firData.fAttDB= 60;
    QFir fir(firData);
    int reduction = fir.GetFilterReduction();
    QVector kernelout;
    fir.Filter(kernel,kernelout);
    for(size_t i = reduction/2; i < fSize-reduction/2; i++) kernel[i]=kernelout[i-reduction/2];
    if(fDebugOn) {
        fDebugVectors.push_back(kernel); // 2 filter in TD low-pass filtered
    }


    // force to zero the kernel at the boundaries
    QVector window2 = QFFT::GetWindow(QFFT::WT_Cosinus,fEffectiveFilterLength,0,false, fMaxJitter);
    window2 = QFFT::ZeroPad(window2,fSize-fEffectiveFilterLength, QFFT::kSymmetric,0);
    fValidSample.resize(fSize);
    for(size_t i = 0; i < fSize; i++) {
        kernel[i] *= window2[i];
        // store the information on the samples that are correctly filtered
        fValidSample[i] = (window2[i] == 0);
    }

    // force to zero the integral of the survived kernel
    double integral = kernel.Sum(fSize,0);

    // alternate method
    /*
       double add = integral/fEffectiveFilterLength;
       add *= fEffectiveFilterLength/window2.Sum(fSize,0);
       for(size_t i = 0; i < fSize; i++) {
       kernel[i] -= add*window2[i];
       }
       */

    double add = 0;
    for(size_t i = 0; i < fSize; i++) {
        if(kernel[i] < 0) add += kernel[i];
    }
    for(size_t i = 0; i < fSize; i++) {
        if(!fValidSample[i]) {
            if(integral > 0 && kernel[i] < 0)  kernel[i] *= (add-integral)/add;
            else if(integral < 0 && kernel[i] > 0)  kernel[i] *= add/(add-integral);
        }
    }


    fTransformer->TransformToFreq(kernel,fFilter1);

    // force to zero the filter integral (even if it should be aready zero)
//    fFilter1[0].Set(0.,0.);
    fFilter1[fSize/2].SetIm(0.);

    // Adjust gain since we manipulated the kernel
    err = Filter(fAveragePulse1);
    if(err != QERR_SUCCESS) return err;
    fCheckForFilteredSamples = QERR_UNKNOWN_ERR;
    fCheckForFilteredSamples.SetDescription(__FILE__,__LINE__,"Filtered samples not available");
    fFilter1 /= fFiltered1.GetMax();


    // Store filter in TD
    fTransformer->TransformFromFreq(fFilter1,fFilterTD);

    // estimate filtered average noise and resolution
    fFilteredAverageNoise = fFilter1.GetModulusSquare().Mult(averageNoise); 
    fFilteredRMS1 = sqrt(fFilteredAverageNoise.Sum(fSize-1,1))/fSize;

    // estimate shifted ap spectra for chi2 evaluation
    //
    fAverageNoiseForChi2=averageNoise;
    fAveragePulse1Shifted.resize(fSize);
    for(size_t j = 0; j < fSize; j++) {
        if(fValidSample[j]) { 
            QVector ap1Shifted;
            if(fAveragePulse1Shifted[j].Size() == 0) {
                ap1Shifted = averagePulse;
                ap1Shifted.Shift(j);
                fTransformer->TransformToFreq(ap1Shifted,fAveragePulse1Shifted[j]);
            }
            if(j+1 < fSize && fAveragePulse1Shifted[j+1].Size() == 0) {
                ap1Shifted = averagePulse;
                ap1Shifted.Shift(j+1);
                fTransformer->TransformToFreq(ap1Shifted,fAveragePulse1Shifted[j+1]);
            }
            if(j > 0 && fAveragePulse1Shifted[j-1].Size() == 0) {
                ap1Shifted = averagePulse;
                ap1Shifted.Shift(j-1);
                fTransformer->TransformToFreq(ap1Shifted,fAveragePulse1Shifted[j-1]);
            }

        }
    }

    return err;

}

QError QOptimumFilter::SetJitter(JitterMode mode, int triggerpos) 
{
    QError err = QERR_SUCCESS;
    switch(mode) {
        case J_ABSMAX: 
            fOptimumJitter = fFiltered1.GetMaxIndex();
            break;
        case J_LOCMAX: 
            {
                // find first local maximum after the trigger. Copied from OptimumFilter.cc
                const int first_start = (triggerpos-fMaxPos+fSize)%fSize;
                int start = first_start;
                bool check = false;
                do {
                    if(fFiltered1[start%fSize]> fFiltered1[(start-1+fSize)%fSize] 
                            && fFiltered1[start%fSize]> fFiltered1[(start+1)%fSize]) {
                        fOptimumJitter = start%fSize; 
                        check=true;
                    }
                    start++;
                }while(!check && (start%(int)fSize) != first_start);
                if(!check) {
                    fOptimumJitter = 0;
                    err= QERR_OUT_OF_RANGE;
                    err.SetDescription(__FILE__,__LINE__,"Failed to find a local maximum, this is wired.");
                }
            }
            break;
        case J_FIXED: 
            /* copied from OptimumFilter.cc, to be checked 
               {
               const int first_start = (triggerpos-fMaxPos+fSize)%fSize;
               const int fixed_guess = (first_start + trigger_shift_ + fSize)%fSize;
               int ii = fixed_guess, maxpos = -1;
               bool check = false;
            // go fixed distance, then move to maximum according to slope 
            while(!check && ii!=first_start)
            {
            const int slope = 
            int(pSamples[ii]>pSamples[(ii-1+fSize)%fSize]) - 
            int(pSamples[ii]>pSamples[(ii+1)%fSize]);
            if(slope==0 && pSamples[ii]<=pSamples[(ii-1+fSize)%fSize])
            {
            // Local min; either have local max on left, or wave is 
            //  decreasing from trigger to ii; either way, move left 
            ii = ((ii - 1 + fSize)%fSize);
            continue;
            }
            if(slope==0)
            {
            check = true;
            maxpos = ii;
            }
            ii = ((ii + slope + fSize)%fSize);
            }
            // If we went back to the trigger, look for the first maximum 
            if(!check)
            {
            ii=first_start;
            do
            {
            if(pSamples[ii]>pSamples[(ii+1)%fSize] && 
            pSamples[ii]>pSamples[(ii-1+fSize)%fSize])
            {
            check = true;
            maxpos = ii;
            }
            ii = ((ii + 1)%fSize);		
            } while(!check && ii!=first_start);
            }
            // If we got to the end of window, set to fixed_guess 
            if(!check)
            {
            if(ii==first_start)
            {
            //QMessageHandler::Debug("OptimumFilter","COF End slope");
            maxpos = avg_nmax;
            }
            else
            QMessageHandler::Panic("OptimumFilter","COF slope something else happened");
            }
            data.max_index = maxpos;
            }
            */
            // MV FIXME: to be implemented
            err= QERR_NOT_IMPLEMENTED;
            err.SetDescription(__FILE__,__LINE__,"Jitter mode to compute amplitude at fixed distance not implmented");
            break;
        case J_NOJITTER: 
            fOptimumJitter = 0;
            break;
        default: 
            err = QERR_OUT_OF_RANGE;
            err.SetDescription(__FILE__,__LINE__,"Jitter mode not implemented");

    }
    if(err != QERR_SUCCESS) return err;

    if(!fValidSample[fOptimumJitter]) {
        err = QERR_OUT_OF_RANGE;
        std::stringstream msg;
        msg<<"Maximum position "<<fOptimumJitter<<" lies outside the filter effective length";
        err.SetDescription(__FILE__,__LINE__,msg.str());
        //std::cout<<"Maximum was: "<<fFiltered1.GetMaxIndex()<<" TriggerPos: "<<triggerpos-fMaxPos<<std::endl;
    }
    return err;

}

QError QOptimumFilter::Get(double& jitter, double& chi2, double& amplitude, double& integral) const
{
    if(fCheckForFilteredSamples != QERR_SUCCESS) return fCheckForFilteredSamples;
    QError err = QERR_SUCCESS;
    jitter = fOptimumJitter;
    if(fOptimumJitter > (int)fSize/2) jitter-=fSize; 
    amplitude = fFiltered1[fOptimumJitter];
    //  std::cout<<fOptimumJitter<<" "<<fAmplitude1<<std::endl;
    const Cuore::QVectorC& ap1f = fAveragePulse1Shifted[fOptimumJitter];
    chi2 = (fTransformedWaveform-amplitude*ap1f).GetModulusSquare().Div(fAverageNoiseForChi2).Sum(fSize-1,1);
    chi2 /= (fSize-2);
    integral = fAvgPulseIntegral1*amplitude;

    return err;
}


QVector QOptimumFilter::GetFitFunction(const double jitter, const double a1) const
{
    QVector output = fAveragePulse1*a1;
    output.ShiftReal(jitter);
    return output;
}