Beispiel #1
0
double IIRNotch::processSample(double input){
  bool v = false;
  
  Eigen::Vector3d x_temp ( input , x(0),  x(1) );
  Eigen::Vector3d y_temp (      0, y(0),  y(1) );
  if(v)  std::cout << input << "\n";
  if(v)  std::cout << x_temp.transpose() << " x_temp\n";
  if(v)  std::cout << y_temp.transpose() << " y_temp\n";
  if(v)  std::cout << b.transpose() << " b\n";
  if(v)  std::cout << a.transpose() << " a\n";
  
  if(v){  
    Eigen::Vector3d bit =  x_temp.cross(b);
    std::cout << bit.transpose() << " bit\n";  
  }
  
  
  double output =  (x_temp.dot(b)) -  (y_temp.dot(a));
  double temp_x = x(0);
  x << input , temp_x  ;
  double temp_y = y(0);
  y <<  output, temp_y ; 
  
  if(v)  std::cout << x.transpose() << " x\n";
  if(v)  std::cout << y.transpose() << " y\n\n";
  
  return output;
}
void Conversions::ecef2tgp(double ecef_x, double ecef_y, double ecef_z, double* tgp_x, double* tgp_y, double* tgp_z)
{
	matrix x_l(3,1), _x_ecef(3,1), x_temp(3,1);


	if (originset)
	{
		// ECEF -> LOCAL
		_x_ecef(0,0,ecef_x);
		_x_ecef(1,0,ecef_y);
		_x_ecef(2,0,ecef_z);

	//	std::cout << "_x_ecef = " << _x_ecef << std::endl;
	//	std::cout << "x0_ecef = " << this->x0_ecef << std::endl;

		x_temp = (_x_ecef - x0_ecef); //T_el * 
		x_l = T_el * x_temp;

	//	std::cout << "x_temp = " << x_temp << std::endl;
	//	std::cout << "T_el = " << T_el << std::endl;
	//	std::cout << "x_l = " << x_l << std::endl;

		*tgp_x=x_l.GetElement(0,0);
		*tgp_y=x_l.GetElement(1,0);
		*tgp_z=x_l.GetElement(2,0);
	}
	else
	{
		*tgp_x=*tgp_y=*tgp_z=0.0;
	}
}
Beispiel #3
0
void RK4_step(                          // replaces x(t) by x(t + dt)
    Matrix<double,1>& x,                // solution vector
    double dt,                          // fixed time step
    Matrix<double,1> flow(Matrix<double,1>&))   // derivative vector
{
    int n = x.size();
    Matrix<double,1> f(n), k1(n), k2(n), k3(n), k4(n), x_temp(n);
    f = flow(x);
    for (int i = 0; i < n; i++) {
        k1[i] = dt * f[i];
        x_temp[i] = x[i] + k1[i] / 2;
    }
    f = flow(x_temp);
    for (int i = 0; i < n; i++) {
        k2[i] = dt * f[i];
        x_temp[i] = x[i] + k2[i] / 2;
    }
    f = flow(x_temp);
    for (int i = 0; i < n; i++) {
        k3[i] = dt * f[i];
        x_temp[i] = x[i] + k3[i];
    }
    f = flow(x_temp);
    for (int i = 0; i < n; i++)
        k4[i] = dt * f[i];
    for (int i = 0; i < n; i++)
        x[i] += (k1[i] + 2 * k2[i] + 2 * k3[i] + k4[i]) / 6;
}
Beispiel #4
0
void Correlation::addResultCurve()
{
    ApplicationWindow *app = dynamic_cast<ApplicationWindow *>(parent());
    if (!app)
        return;

    QLocale locale = app->locale();

    if (d_n > d_table->numRows())
        d_table->setNumRows(d_n);

	int cols = d_table->numCols();
	int cols2 = cols+1;
	d_table->addCol();
	d_table->addCol();
	int n = d_n/2;

	QVarLengthArray<double> x_temp(d_n), y_temp(d_n);//double x_temp[d_n], y_temp[d_n];
	for (int i = 0; i<d_n; i++){
	    double x = i - n;
        x_temp[i] = x;

        double y;
        if(i < n)
			y = d_x[n + i];
		else
			y = d_x[i - n];
        y_temp[i] = y;

		d_table->setText(i, cols, QString::number(x));
		d_table->setText(i, cols2, locale.toString(y, 'g', app->d_decimal_digits));
	}

	QStringList l = d_table->colNames().grep(tr("Lag"));
	QString id = QString::number((int)l.size()+1);
	QString label = objectName() + id;

	d_table->setColName(cols, tr("Lag") + id);
	d_table->setColName(cols2, label);
	d_table->setColPlotDesignation(cols, Table::X);
	d_table->setHeaderColType();

	if (d_graphics_display){
		if (!d_output_graph)
			d_output_graph = createOutputGraph()->activeGraph();

    	DataCurve *c = new DataCurve(d_table, d_table->colName(cols), d_table->colName(cols2));
		c->setData(x_temp.data(), y_temp.data(), d_n);//c->setData(x_temp, y_temp, d_n);
    	c->setPen(QPen(ColorBox::color(d_curveColorIndex), 1));
		d_output_graph->insertPlotItem(c, Graph::Line);
		d_output_graph->updatePlot();
	}
}
void Convolution::addResultCurve()
{
    ApplicationWindow *app = (ApplicationWindow *)parent();
    if (!app)
        return;

	int cols = d_table->numCols();
	int cols2 = cols+1;

	d_table->addCol();
	d_table->addCol();
#ifdef Q_CC_MSVC
    QVarLengthArray<double> x_temp(d_n);
#else
    double x_temp[d_n];
#endif
	QLocale locale = app->locale();
	for (int i = 0; i<d_n; i++){
		double x = i+1;
		x_temp[i] = x;

		d_table->setText(i, cols, QString::number(x));
		d_table->setText(i, cols2, locale.toString(d_x[i], 'g', app->d_decimal_digits));
	}

	QStringList l = d_table->colNames().grep(tr("Index"));
	QString id = QString::number((int)l.size()+1);
	QString label = objectName() + id;

	d_table->setColName(cols, tr("Index") + id);
	d_table->setColName(cols2, label);
	d_table->setColPlotDesignation(cols, Table::X);
	d_table->setHeaderColType();

	if (d_graphics_display){
		if (!d_output_graph)
			createOutputGraph();

    	DataCurve *c = new DataCurve(d_table, d_table->colName(cols), d_table->colName(cols2));
#ifdef Q_CC_MSVC
		c->setData(x_temp.data(), d_x, d_n);
#else
		c->setData(x_temp, d_x, d_n);
#endif
    	c->setPen(QPen(ColorBox::color(d_curveColorIndex), 1));
		d_output_graph->insertPlotItem(c, Graph::Line);
		d_output_graph->updatePlot();
	}
}
      void least_squares_eqcon(Eigen::MatrixBase<data1__>  &x, const Eigen::MatrixBase<data2__> &A, const Eigen::MatrixBase<data3__> &b, const Eigen::MatrixBase<data4__> &B, const Eigen::MatrixBase<data5__> &d)
      {
        typedef Eigen::Matrix<typename Eigen::MatrixBase<data4__>::Scalar, Eigen::Dynamic, Eigen::Dynamic> B_matrixD;
        B_matrixD Q, R, temp_B, temp_R;
        typedef Eigen::Matrix<typename Eigen::MatrixBase<data2__>::Scalar, Eigen::Dynamic, Eigen::Dynamic> A_matrixD;
        A_matrixD A1, A2, temp_A;
        typedef Eigen::Matrix<typename Eigen::MatrixBase<data5__>::Scalar, Eigen::Dynamic, Eigen::Dynamic> d_matrixD;
        d_matrixD y;
        typedef Eigen::Matrix<typename Eigen::MatrixBase<data3__>::Scalar, Eigen::Dynamic, Eigen::Dynamic> b_matrixD;
        b_matrixD z, rhs;
        typedef Eigen::Matrix<typename Eigen::MatrixBase<data1__>::Scalar, Eigen::Dynamic, Eigen::Dynamic> x_matrixD;
        x_matrixD x_temp(A.cols(), b.cols());
        typename A_matrixD::Index p(B.rows()), n(A.cols());
  #ifdef DEBUG
        typename A_matrixD::Index m(b.rows());
  #endif

        // build Q and R
        Eigen::HouseholderQR<B_matrixD> qr(B.transpose());
        Q=qr.householderQ();
        temp_B=qr.matrixQR();
        temp_R=Eigen::TriangularView<B_matrixD, Eigen::Upper>(temp_B);
        R=temp_R.topRows(p);
        assert((R.rows()==p) && (R.cols()==p));
        assert((Q.rows()==n) && (Q.cols()==n));

        // build A1 and A2
        temp_A=A*Q;
        A1=temp_A.leftCols(p);
        A2=temp_A.rightCols(n-p);
#ifdef DEBUG
        assert((A1.rows()==m) && (A1.cols()==p));
        assert((A2.rows()==m) && (A2.cols()==n-p));
#endif
        assert(A1.cols()==p);
        assert(A2.cols()==n-p);

        // solve for y
        y=R.transpose().lu().solve(d);

        // setup the unconstrained optimization
        rhs=b-A1*y;
        least_squares_uncon(z, A2, rhs);

        // build the solution
        x_temp.topRows(p)=y;
        x_temp.bottomRows(n-p)=z;
        x=Q*x_temp;
      }
Beispiel #7
0
    std::vector<
            typename viennacl::result_of::cpu_value_type<typename VectorT::value_type>::type
            > 
    bisect(VectorT const & alphas, VectorT const & betas)
    {
      typedef typename viennacl::result_of::value_type<VectorT>::type           ScalarType;
      typedef typename viennacl::result_of::cpu_value_type<ScalarType>::type    CPU_ScalarType;  

      std::size_t size = betas.size();
      std::vector<CPU_ScalarType>  x_temp(size);


      std::vector<CPU_ScalarType> beta_bisect;
      std::vector<CPU_ScalarType> wu;

      double rel_error = std::numeric_limits<CPU_ScalarType>::epsilon();
      beta_bisect.push_back(0);
    
      for(std::size_t i = 1; i < size; i++){
              beta_bisect.push_back(betas[i] * betas[i]);
      }

      double xmin = alphas[size - 1] - std::fabs(betas[size - 1]);
      double xmax = alphas[size - 1] + std::fabs(betas[size - 1]);

      for(std::size_t i = 0; i < size - 1; i++)
      {
        double h = std::fabs(betas[i]) + std::fabs(betas[i + 1]);
        if (alphas[i] + h > xmax)
          xmax = alphas[i] + h;
        if (alphas[i] - h < xmin)
          xmin = alphas[i] - h;
      }

      
      double eps1 = 1e-6;
      /*double eps2 = (xmin + xmax > 0) ? (rel_error * xmax) : (-rel_error * xmin);
      if(eps1 <= 0)
        eps1 = eps2;
      else
        eps2 = 0.5 * eps1 + 7.0 * eps2; */

      double x0 = xmax;

      for(std::size_t i = 0; i < size; i++)
      {
        x_temp[i] = xmax;
        wu.push_back(xmin);
      }

      for(long k = size - 1; k >= 0; --k)
      {
        double xu = xmin;
        for(long i = k; i >= 0; --i)
        {
          if(xu < wu[k-i])
          {
            xu = wu[i];
            break;
          }
        }
        
        if(x0 > x_temp[k])
          x0 = x_temp[k];

        double x1 = (xu + x0) / 2.0;
        while (x0 - xu > 2.0 * rel_error * (std::fabs(xu) + std::fabs(x0)) + eps1)
        {
          std::size_t a = 0;
          double q = 1;
          for(std::size_t i = 0; i < size; i++)
          {
            if(q != 0)
              q = alphas[i] - x1 - beta_bisect[i] / q;
            else
              q = alphas[i] - x1 - std::fabs(betas[i] / rel_error);

            if(q < 0)
              a++;
          }
          
          if (a <= static_cast<std::size_t>(k))
          {
            xu = x1;
            if(a < 1)
              wu[0] = x1;
            else
            {
              wu[a] = x1;
              if(x_temp[a - 1] > x1)
                  x_temp[a - 1] = x1;
            }
          }
          else
            x0 = x1;

          x1 = (xu + x0) / 2.0;
        }
        x_temp[k] = x1;
      }
      return x_temp;
    }
Beispiel #8
0
RcppExport SEXP readBeadFindMask(SEXP beadFindFile_in, SEXP x_in, SEXP y_in) {

    SEXP rl = R_NilValue; 		// Use this when there is nothing to be returned.
    char *exceptionMesg = NULL;

    try {

	// Recasting of input arguments
	Rcpp::StringVector  beadFindFile_temp(beadFindFile_in);
	std::string *beadFindFile = new std::string(beadFindFile_temp(0));
	// x
	Rcpp::IntegerVector x_temp(x_in);
	uint64_t nX = x_temp.size();
	Rcpp::IntegerVector x(nX);
	int xMin = INT_MAX;
	int xMax = -1;
	int newVal = 0;
	for(uint64_t i=0; i<nX; i++) {
	    newVal = x_temp(i);
	    x(i) = newVal;
	    if(newVal < xMin)
		xMin = newVal;
	    if(newVal > xMax)
		xMax = newVal;
	}
	// y
	Rcpp::IntegerVector y_temp(y_in);
	uint64_t nY = y_temp.size();
	Rcpp::IntegerVector y(nX);
	int yMin = INT_MAX;
	int yMax = -1;
	for(uint64_t i=0; i<nY; i++) {
	    newVal = y_temp(i);
	    y(i) = newVal;
	    if(newVal < yMin)
		yMin = newVal;
	    if(newVal > yMax)
		yMax = newVal;
	}
 
	// Ensure lower bounds are positive and less than the upper bounds, then proceed to open the file
	if(nX != nY) {
	    exceptionMesg = strdup("x and y should be of the same length");
	} else if(nX < 0) {
	    exceptionMesg = strdup("x and y should be of positive length");
	} else if(xMin < 0) {
	    exceptionMesg = strdup("xMin must be positive");
	} else if(yMin < 0) {
	    exceptionMesg = strdup("yMin must be positive");
	} else if(xMin > xMax) {
	    exceptionMesg = strdup("xMin must be less than xMax");
	} else if(yMin > yMax) {
	    exceptionMesg = strdup("yMin must be less than yMax");
	} else {
	    FILE *fp = NULL;
	    fp = fopen(beadFindFile->c_str(),"rb");
	    if(!fp) {
		std::string exception = "unable to open beadFindFile " + *beadFindFile;
		exceptionMesg = strdup(exception.c_str());
	    } else {
		int32_t nRow = 0;
		int32_t nCol = 0;
    
		if ((fread (&nRow, sizeof(uint32_t), 1, fp )) != 1) {
		    // Read number of rows (aka "y" or "height")
		    std::string exception = "Problem reading nRow from beadFindFile" + *beadFindFile;
		    exceptionMesg = strdup(exception.c_str());
		} else if ((fread (&nCol, sizeof(uint32_t), 1, fp )) != 1) {
		    // Read number of cols (aka "x" or "width")
		    std::string exception = "Problem reading nCol from beadFindFile" + *beadFindFile;
		    exceptionMesg = strdup(exception.c_str());
		} else {
		    // Ensure upper bounds are within range before continuing
		    if(yMax >= nRow) {
			exceptionMesg = strdup("yMax must be less than the number of rows");
		    } else if(xMax >= nCol) {
			exceptionMesg = strdup("xMax must be less than the number of cols");
		    } else {
			// Iterate over mask data and store out the encoded Boolean values
			uint16_t mask = 0;
			Rcpp::IntegerVector maskEmpty(nX);
			Rcpp::IntegerVector maskBead(nX);
			Rcpp::IntegerVector maskLive(nX);
			Rcpp::IntegerVector maskDud(nX);
			Rcpp::IntegerVector maskReference(nX);
			Rcpp::IntegerVector maskTF(nX);
			Rcpp::IntegerVector maskLib(nX);
			Rcpp::IntegerVector maskPinned(nX);
			Rcpp::IntegerVector maskIgnore(nX);
			Rcpp::IntegerVector maskWashout(nX);
			Rcpp::IntegerVector maskExclude(nX);
			Rcpp::IntegerVector maskKeypass(nX);
			Rcpp::IntegerVector maskFilteredBadKey(nX);
			Rcpp::IntegerVector maskFilteredShort(nX);
			Rcpp::IntegerVector maskFilteredBadPPF(nX);
			Rcpp::IntegerVector maskFilteredBadResidual(nX);
			int64_t headerSkipBytes = 2 * sizeof(uint32_t);
			int64_t dataSkipBytes = sizeof(uint16_t);
			int64_t offset;
			for (uint64_t i=0; i < nX; i++) {
			    int col = x(i); 
			    int row = y(i);
			    offset = headerSkipBytes + (row * nCol + col) * dataSkipBytes;
			    fseek(fp, offset, SEEK_SET);
			    if ((fread (&mask, sizeof(uint16_t), 1, fp)) != 1) {
				std::string exception = "Problem reading mask values from " + *beadFindFile;
				exceptionMesg = strdup(exception.c_str());
				break;
			    } else {
				maskEmpty(i)                = (mask & MaskEmpty) > 0;
				maskBead(i)                 = (mask & MaskBead) > 0;
				maskLive(i)                 = (mask & MaskLive) > 0;
				maskDud(i)                  = (mask & MaskDud) > 0;
				maskReference(i)            = (mask & MaskReference) > 0;
				maskTF(i)                   = (mask & MaskTF) > 0;
				maskLib(i)                  = (mask & MaskLib) > 0;
				maskPinned(i)               = (mask & MaskPinned) > 0;
				maskIgnore(i)               = (mask & MaskIgnore) > 0;
				maskWashout(i)              = (mask & MaskWashout) > 0;
				maskExclude(i)              = (mask & MaskExclude) > 0;
				maskKeypass(i)              = (mask & MaskKeypass) > 0;
				maskFilteredBadKey(i)       = (mask & MaskFilteredBadKey) > 0;
				maskFilteredShort(i)        = (mask & MaskFilteredShort) > 0;
				maskFilteredBadPPF(i)       = (mask & MaskFilteredBadPPF) > 0;
				maskFilteredBadResidual(i)  = (mask & MaskFilteredBadResidual) > 0;
			    }
			}

			// Check if there are any non-zero values for Keypass and Exclude and filtered data.
			// If not then it is likely that this is a legacy bfmask.bin file and we won't return
			// these fields.
			//bool haveFiltered=0;
			//for (uint64_t i=0; i < nX; i++) {
			//    if(maskKeypass(i) || maskExclude(i) || maskFilteredBadKey(i) || maskFilteredShort(i) ||
            //       maskFilteredBadPPF(i) || maskFilteredBadResidual(i)) {
			//	haveFiltered=1;
			//	break;
			//    }
			//}

			// Build result set to be returned as a list to R.
            std::map<std::string,SEXP> map;
            map["beadFindMaskFile"] = Rcpp::wrap( *beadFindFile );
            map["nCol"]             = Rcpp::wrap( nCol );
            map["nRow"]             = Rcpp::wrap( nRow );
            map["col"]              = Rcpp::wrap( x );
            map["row"]              = Rcpp::wrap( y );
            map["maskEmpty"]        = Rcpp::wrap( maskEmpty );
            map["maskBead"]         = Rcpp::wrap( maskBead );
            map["maskLive"]         = Rcpp::wrap( maskLive );
            map["maskDud"]          = Rcpp::wrap( maskDud );
            map["maskReference"]    = Rcpp::wrap( maskReference );
            map["maskTF"]           = Rcpp::wrap( maskTF );
            map["maskLib"]          = Rcpp::wrap( maskLib );
            map["maskPinned"]       = Rcpp::wrap( maskPinned );
            map["maskIgnore"]       = Rcpp::wrap( maskIgnore );
            map["maskWashout"]      = Rcpp::wrap( maskWashout );
			//if(haveFiltered) {
              map["maskExclude"]              = Rcpp::wrap( maskExclude );
              map["maskKeypass"]              = Rcpp::wrap( maskKeypass );
              map["maskFilteredBadKey"]       = Rcpp::wrap( maskFilteredBadKey );
              map["maskFilteredShort"]        = Rcpp::wrap( maskFilteredShort );
              map["maskFilteredBadPPF"]       = Rcpp::wrap( maskFilteredBadPPF );
              map["maskFilteredBadResidual"]  = Rcpp::wrap( maskFilteredBadResidual );
            //}


			// Get the list to be returned to R.
            rl = Rcpp::wrap( map ) ;
		    }
		}

		fclose(fp);
	    }
	}

	delete beadFindFile;
	
    } catch(std::exception& ex) {
	forward_exception_to_r(ex);
    } catch(...) {
	::Rf_error("c++ exception (unknown reason)");
    }
    
    if(exceptionMesg != NULL)
	Rf_error(exceptionMesg);

    return rl;
}