freqest.cpp

/***************************************************************************
        Copyright (C) 2002-2015 Kentaro Kitagawa
                           kitagawa@phys.s.u-tokyo.ac.jp
        
        This program is free software; you can redistribute it and/or
        modify it under the terms of the GNU Library General Public
        License as published by the Free Software Foundation; either
        version 2 of the License, or (at your option) any later version.
        
        You should have received a copy of the GNU Library General 
        Public License and a list of authors along with this program; 
        see the files COPYING and AUTHORS.
***************************************************************************/
#include "freqest.h"

#ifdef HAVE_LAPACK

#include "matrix.h"
#include <numeric>
#include <boost/numeric/ublas/matrix_proxy.hpp>
#include <boost/numeric/ublas/io.hpp>

void
FreqEstimation::genSpectrum(const std::vector<std::complex<double> >& memin,
        std::vector<std::complex<double> >& memout,
        int t0, double tol, FFT::twindowfunc windowfunc, double windowlength) {
    int t = memin.size();
    int n = memout.size();
    
    if(t > 1024)
        throw XKameError(i18n("Too large size to allocate a matrix."), __FILE__, __LINE__);
    
    int wpoints = lrint(numberOfNoises(memin)); //# of fittable data in freq. domain.
    wpoints = std::min(std::max(wpoints, t/100 + 1), t);
//  fprintf(stderr, "# of data points = %d\n", wpoints);
    
    double tpoworg = 0.0;
    for(int i = 0; i < t; i++) {
        tpoworg += std::norm(memin[i]);
    }

    std::vector<std::complex<double> > rx(t);
    autoCorrelation(memin, rx);
    //# of signal space.
    int p = t; // / 2 - 1;
    rx.resize(p);
    // Correlation matrix.
    ublas::matrix<std::complex<double> > r(p, p);
    for(int i = 0; i < p; i++) {
        ublas::matrix_row<ublas::matrix<std::complex<double> > > rrow(r, i);
        for(int j = i; j < p; j++) {
            rrow(j) = rx[j - i];
        }
    }
    ublas::matrix<std::complex<double> > eigv;
    ublas::vector<double> lambda;
    double tol_lambda = tol * std::abs(rx[0]) * 0.1;
    eigHermiteRRR(r, lambda, eigv, tol_lambda);

    //# of signals.
    int numsig = 0;
    if(!m_mvdl_method) {
        //Minimum IC.
        double minic = 1e99;
        double sumlambda = 0.0, sumloglambda = 1.0;
        for(int i = 0; i < p; i++) {
            sumlambda += lambda[i];
            sumloglambda += log(lambda[i]);
            int q = p - 1 - i;
            double logl = t * (p - q) * (sumloglambda / (double)(p - q) - log(sumlambda / (double)(p - q)));
            double ic = m_funcIC(logl, q * (2*p - q), wpoints);
            if(ic < minic) {
                minic = ic;
                numsig = q;
            }
        }
        numsig *= windowlength;
        numsig = std::max(std::min(numsig, p - 1), 0);
        
//      fprintf(stderr, "MinIC=%g, # of signal=%d, p=%d\n", minic, numsig, p);

//      std::cout << lambda << std::endl;
//      std::cout << eigv << std::endl;
    }
    std::vector<std::complex<double> > fftin(t, 0.0), fftout(t), acsum(t, 0.0);
    for(int i = 0; i < p - numsig; i++) {
        ublas::matrix_column<ublas::matrix<std::complex<double> > > eigvcol(eigv, i);
        assert(fabs(norm_2(eigvcol) - 1.0) < 0.1);
        for(int j = 0; j < p; j++) {
            fftin[j] = eigvcol(j);
        }
        autoCorrelation(fftin, fftout);
        double z = lambda[i];
        z = std::max(z, tol_lambda);
        z = (m_eigenvalue_method) ? (1.0 / z) : 1.0;
        for(int k = 0; k < p; k++) {
            acsum[k] += fftout[k] * z;
        }
    }
    std::vector<std::complex<double> > zffftin(n, 0.0), zffftout(n);
    std::vector<double> ip(n), dy(n);
    acsum[0] /= (double)2;
    std::copy(acsum.begin(), acsum.end(), zffftin.begin());
    m_ifftN->exec(zffftin, zffftout);
    for(int i = 0; i < n; i++)
        ip[i] = std::real(zffftout[i]);
    for(int i = 0; i < p; i++)
        zffftin[i] = (double)((i >= n/2) ? (i - n) : i) * acsum[i] * std::complex<double>(0, 1);
    m_ifftN->exec(zffftin, zffftout);
    for(int i = 0; i < n; i++)
        dy[i] = std::real(zffftout[i]);
    
    //Power spectrum density.
    std::vector<double> psd(n);
    double tpow = 0.0;
    for(int i = 0; i < n; i++) {
        double z = 1.0 / ip[i];
        tpow += z;
        psd[i] = z;
    }
    double normalize = n * tpoworg / tpow;
    for(int i = 0; i < n; i++) {
        psd[i] *= normalize;
    }
    //Least-Square Phase Estimation.
    double coeff = lspe(memin, t0, psd, memout, tol, true, windowfunc); 
    normalize *= coeff * coeff;
    
    //Peak detection. Sub-resolution detection for smooth curves.
    for(int ip = 0; ip < n; ip++) {
        int in = (ip + 1) % n;
        if((dy[ip] < 0) && (dy[in] > 0)) {
            double dx = - dy[ip] / (dy[in] - dy[ip]);
            if((dx >= 0) && (dx <= 1.0)) {
                std::complex<double> z = 0.0, xn = 1.0,
                    x = std::polar(1.0, 2 * M_PI * (dx + ip) / (double)n);
                for(int j = 0; j < p; j++) {
                    z += acsum[j] * xn;
                    xn *= x;
                }
                double r = sqrt(normalize * std::max(1.0 / std::real(z), 0.0));
                m_peaks.push_back(std::pair<double, double>(r, dx + ip));
            }
        }
    }
}
#endif// HAVE_LAPACK