void Bernsteins::find_bernstein_roots(Bezier bz, unsigned depth, double left_t, double right_t) { debug(std::cout << left_t << ", " << right_t << std::endl); size_t n_crossings = 0; int old_sign = SGN(bz[0]); //std::cout << "w[0] = " << bz[0] << std::endl; int sign; for (size_t i = 1; i < bz.size(); i++) { //std::cout << "w[" << i << "] = " << w[i] << std::endl; sign = SGN(bz[i]); if (sign != 0) { if (sign != old_sign && old_sign != 0) { ++n_crossings; } old_sign = sign; } } //std::cout << "n_crossings = " << n_crossings << std::endl; if (n_crossings == 0) return; // no solutions here if (n_crossings == 1) /* Unique solution */ { //std::cout << "depth = " << depth << std::endl; /* Stop recursion when the tree is deep enough */ /* if deep enough, return 1 solution at midpoint */ if (depth > MAX_DEPTH) { //printf("bottom out %d\n", depth); const double Ax = right_t - left_t; const double Ay = bz.at1() - bz.at0(); solutions.push_back(left_t - Ax*bz.at0() / Ay); return; } double r = secant(bz); solutions.push_back(r*right_t + (1-r)*left_t); return; } /* Otherwise, solve recursively after subdividing control polygon */ Bezier::Order o(bz); Bezier Left(o), Right = bz; double split_t = (left_t + right_t) * 0.5; // If subdivision is working poorly, split around the leftmost root of the derivative if (depth > 2) { debug(std::cout << "derivative mode\n"); Bezier dbz = derivative(bz); debug(std::cout << "initial = " << dbz << std::endl); std::vector<double> dsolutions = dbz.roots(Interval(left_t, right_t)); debug(std::cout << "dsolutions = " << dsolutions << std::endl); double dsplit_t = 0.5; if(!dsolutions.empty()) { dsplit_t = dsolutions[0]; split_t = left_t + (right_t - left_t)*dsplit_t; debug(std::cout << "split_value = " << bz(split_t) << std::endl); debug(std::cout << "spliting around " << dsplit_t << " = " << split_t << "\n"); } std::pair<Bezier, Bezier> LR = bz.subdivide(dsplit_t); Left = LR.first; Right = LR.second; } else { // split at midpoint, because it is cheap Left[0] = Right[0]; for (size_t i = 1; i < bz.size(); ++i) { for (size_t j = 0; j < bz.size()-i; ++j) { Right[j] = (Right[j] + Right[j+1]) * 0.5; } Left[i] = Right[0]; } } debug(std::cout << "Solution is exactly on the subdivision point.\n"); debug(std::cout << Left << " , " << Right << std::endl); Left = reverse(Left); while(Right.order() > 0 and fabs(Right[0]) <= 1e-10) { debug(std::cout << "deflate\n"); Right = Right.deflate(); Left = Left.deflate(); solutions.push_back(split_t); } Left = reverse(Left); if (Right.order() > 0) { debug(std::cout << Left << " , " << Right << std::endl); find_bernstein_roots(Left, depth+1, left_t, split_t); find_bernstein_roots(Right, depth+1, split_t, right_t); } }
/** * \brief returns all the parameter values of A whose tangent passes through P. * \relates D2 */ std::vector<double> find_tangents(Point P, D2<SBasis> const &A) { SBasis crs (cross(A - P, derivative(A))); return roots(crs); }
//---------------------------------------------------------------------------------------------- /// Examine the chi squared as a function of fitting parameters and estimate /// errors for each parameter. void CalculateChiSquared::estimateErrors() { // Number of fiting parameters auto nParams = m_function->nParams(); // Create an output table for displaying slices of the chi squared and // the probabilitydensity function auto pdfTable = API::WorkspaceFactory::Instance().createTable(); std::string baseName = getProperty("Output"); if (baseName.empty()) { baseName = "CalculateChiSquared"; } declareProperty(new API::WorkspaceProperty<API::ITableWorkspace>( "PDFs", "", Kernel::Direction::Output), "The name of the TableWorkspace in which to store the " "pdfs of fit parameters"); setPropertyValue("PDFs", baseName + "_pdf"); setProperty("PDFs", pdfTable); // Create an output table for displaying the parameter errors. auto errorsTable = API::WorkspaceFactory::Instance().createTable(); auto nameColumn = errorsTable->addColumn("str", "Parameter"); auto valueColumn = errorsTable->addColumn("double", "Value"); auto minValueColumn = errorsTable->addColumn("double", "Value at Min"); auto leftErrColumn = errorsTable->addColumn("double", "Left Error"); auto rightErrColumn = errorsTable->addColumn("double", "Right Error"); auto quadraticErrColumn = errorsTable->addColumn("double", "Quadratic Error"); auto chiMinColumn = errorsTable->addColumn("double", "Chi2 Min"); errorsTable->setRowCount(nParams); declareProperty(new API::WorkspaceProperty<API::ITableWorkspace>( "Errors", "", Kernel::Direction::Output), "The name of the TableWorkspace in which to store the " "values and errors of fit parameters"); setPropertyValue("Errors", baseName + "_errors"); setProperty("Errors", errorsTable); // Calculate initial values double chiSquared = 0.0; double chiSquaredWeighted = 0.0; double dof = 0; API::FunctionDomain_sptr domain; API::FunctionValues_sptr values; m_domainCreator->createDomain(domain, values); calcChiSquared(*m_function, nParams, *domain, *values, chiSquared, chiSquaredWeighted, dof); // Value of chi squared for current parameters in m_function double chi0 = chiSquared; // Fit data variance double sigma2 = chiSquared / dof; bool useWeighted = getProperty("Weighted"); if (useWeighted) { chi0 = chiSquaredWeighted; sigma2 = 0.0; } if (g_log.is(Kernel::Logger::Priority::PRIO_DEBUG)) { g_log.debug() << "chi0=" << chi0 << std::endl; g_log.debug() << "sigma2=" << sigma2 << std::endl; g_log.debug() << "dof=" << dof << std::endl; } // Parameter bounds that define a volume in the parameter // space within which the chi squared is being examined. GSLVector lBounds(nParams); GSLVector rBounds(nParams); // Number of points in lines for plotting size_t n = 100; pdfTable->setRowCount(n); const double fac = 1e-4; // Loop over each parameter for (size_t ip = 0; ip < nParams; ++ip) { // Add columns for the parameter to the pdf table. auto parName = m_function->parameterName(ip); nameColumn->read(ip, parName); // Parameter values auto col1 = pdfTable->addColumn("double", parName); col1->setPlotType(1); // Chi squared values auto col2 = pdfTable->addColumn("double", parName + "_chi2"); col2->setPlotType(2); // PDF values auto col3 = pdfTable->addColumn("double", parName + "_pdf"); col3->setPlotType(2); double par0 = m_function->getParameter(ip); double shift = fabs(par0 * fac); if (shift == 0.0) { shift = fac; } // Make a slice along this parameter GSLVector dir(nParams); dir.zero(); dir[ip] = 1.0; ChiSlice slice(*m_function, dir, *domain, *values, chi0, sigma2); // Find the bounds withn which the PDF is significantly above zero. // The bounds are defined relative to par0: // par0 + lBound is the lowest value of the parameter (lBound <= 0) // par0 + rBound is the highest value of the parameter (rBound >= 0) double lBound = slice.findBound(-shift); double rBound = slice.findBound(shift); lBounds[ip] = lBound; rBounds[ip] = rBound; // Approximate the slice with a polynomial. // P is a vector of values of the polynomial at special points. // A is a vector of Chebyshev expansion coefficients. // The polynomial is defined on interval [lBound, rBound] // The value of the polynomial at 0 == chi squared at par0 std::vector<double> P, A; bool ok = true; auto base = slice.makeApprox(lBound, rBound, P, A, ok); if (!ok) { g_log.warning() << "Approximation failed for parameter " << ip << std::endl; } if (g_log.is(Kernel::Logger::Priority::PRIO_DEBUG)) { g_log.debug() << "Parameter " << ip << std::endl; g_log.debug() << "Slice approximated by polynomial of order " << base->size() - 1; g_log.debug() << " between " << lBound << " and " << rBound << std::endl; } // Write n slice points into the output table. double dp = (rBound - lBound) / static_cast<double>(n); for (size_t i = 0; i < n; ++i) { double par = lBound + dp * static_cast<double>(i); double chi = base->eval(par, P); col1->fromDouble(i, par0 + par); col2->fromDouble(i, chi); } // Check if par0 is a minimum point of the chi squared std::vector<double> AD; // Calculate the derivative polynomial. // AD are the Chebyshev expansion of the derivative. base->derivative(A, AD); // Find the roots of the derivative polynomial std::vector<double> minima = base->roots(AD); if (minima.empty()) { minima.push_back(par0); } if (g_log.is(Kernel::Logger::Priority::PRIO_DEBUG)) { g_log.debug() << "Minima: "; } // If only 1 extremum is found assume (without checking) that it's a // minimum. // If there are more than 1, find the one with the smallest chi^2. double chiMin = std::numeric_limits<double>::max(); double parMin = par0; for (size_t i = 0; i < minima.size(); ++i) { double value = base->eval(minima[i], P); if (g_log.is(Kernel::Logger::Priority::PRIO_DEBUG)) { g_log.debug() << minima[i] << " (" << value << ") "; } if (value < chiMin) { chiMin = value; parMin = minima[i]; } } if (g_log.is(Kernel::Logger::Priority::PRIO_DEBUG)) { g_log.debug() << std::endl; g_log.debug() << "Smallest minimum at " << parMin << " is " << chiMin << std::endl; } // Points of intersections with line chi^2 = 1/2 give an estimate of // the standard deviation of this parameter if it's uncorrelated with the // others. A[0] -= 0.5; // Now A are the coefficients of the original polynomial // shifted down by 1/2. std::vector<double> roots = base->roots(A); std::sort(roots.begin(), roots.end()); if (roots.empty()) { // Something went wrong; use the whole interval. roots.resize(2); roots[0] = lBound; roots[1] = rBound; } else if (roots.size() == 1) { // Only one root found; use a bound for the other root. if (roots.front() < 0) { roots.push_back(rBound); } else { roots.insert(roots.begin(), lBound); } } else if (roots.size() > 2) { // More than 2 roots; use the smallest and the biggest auto smallest = roots.front(); auto biggest = roots.back(); roots.resize(2); roots[0] = smallest; roots[1] = biggest; } if (g_log.is(Kernel::Logger::Priority::PRIO_DEBUG)) { g_log.debug() << "Roots: "; for (size_t i = 0; i < roots.size(); ++i) { g_log.debug() << roots[i] << ' '; } g_log.debug() << std::endl; } // Output parameter info to the table. valueColumn->fromDouble(ip, par0); minValueColumn->fromDouble(ip, par0 + parMin); leftErrColumn->fromDouble(ip, roots[0] - parMin); rightErrColumn->fromDouble(ip, roots[1] - parMin); chiMinColumn->fromDouble(ip, chiMin); // Output the PDF for (size_t i = 0; i < n; ++i) { double chi = col2->toDouble(i); col3->fromDouble(i, exp(-chi + chiMin)); } // make sure function parameters don't change. m_function->setParameter(ip, par0); } // Improve estimates for standard deviations. // If parameters are correlated the found deviations // most likely underestimate the true values. unfixParameters(); GSLJacobian J(m_function, values->size()); m_function->functionDeriv(*domain, J); refixParameters(); // Calculate the hessian at the current point. GSLMatrix H; if (useWeighted) { H.resize(nParams, nParams); for (size_t i = 0; i < nParams; ++i) { for (size_t j = i; j < nParams; ++j) { double h = 0.0; for (size_t k = 0; k < values->size(); ++k) { double w = values->getFitWeight(k); h += J.get(k, i) * J.get(k, j) * w * w; } H.set(i, j, h); if (i != j) { H.set(j, i, h); } } } } else { H = Tr(J.matrix()) * J.matrix(); } // Square roots of the diagonals of the covariance matrix give // the standard deviations in the quadratic approximation of the chi^2. GSLMatrix V(H); if (!useWeighted) { V *= 1. / sigma2; } V.invert(); // In a non-quadratic asymmetric case the following procedure can give a // better result: // Find the direction in which the chi^2 changes slowest and the positive and // negative deviations in that direction. The change in a parameter at those // points can be a better estimate for the standard deviation. GSLVector v(nParams); GSLMatrix Q(nParams, nParams); // One of the eigenvectors of the hessian is the direction of the slowest // change. H.eigenSystem(v, Q); // Loop over the eigenvectors for (size_t i = 0; i < nParams; ++i) { auto dir = Q.copyColumn(i); if (g_log.is(Kernel::Logger::Priority::PRIO_DEBUG)) { g_log.debug() << "Direction " << i << std::endl; g_log.debug() << dir << std::endl; } // Make a slice in that direction ChiSlice slice(*m_function, dir, *domain, *values, chi0, sigma2); double rBound0 = dir.dot(rBounds); double lBound0 = dir.dot(lBounds); if (g_log.is(Kernel::Logger::Priority::PRIO_DEBUG)) { g_log.debug() << "lBound " << lBound0 << std::endl; g_log.debug() << "rBound " << rBound0 << std::endl; } double lBound = slice.findBound(lBound0); double rBound = slice.findBound(rBound0); std::vector<double> P, A; // Use a polynomial approximation bool ok = true; auto base = slice.makeApprox(lBound, rBound, P, A, ok); if (!ok) { g_log.warning() << "Approximation failed in direction " << i << std::endl; } // Find the deviation points where the chi^2 = 1/2 A[0] -= 0.5; std::vector<double> roots = base->roots(A); std::sort(roots.begin(), roots.end()); // Sort out the roots auto nRoots = roots.size(); if (nRoots == 0) { roots.resize(2, 0.0); } else if (nRoots == 1) { if (roots.front() > 0.0) { roots.insert(roots.begin(), 0.0); } else { roots.push_back(0.0); } } else if (nRoots > 2) { roots[1] = roots.back(); roots.resize(2); } if (g_log.is(Kernel::Logger::Priority::PRIO_DEBUG)) { g_log.debug() << "Roots " << roots[0] << " (" << slice(roots[0]) << ") " << roots[1] << " (" << slice(roots[1]) << ") " << std::endl; } // Loop over the parameters and see if there deviations along // this direction is greater than any previous value. for (size_t ip = 0; ip < nParams; ++ip) { auto lError = roots.front() * dir[ip]; auto rError = roots.back() * dir[ip]; if (lError > rError) { std::swap(lError, rError); } if (lError < leftErrColumn->toDouble(ip)) { if (g_log.is(Kernel::Logger::Priority::PRIO_DEBUG)) { g_log.debug() << " left for " << ip << ' ' << lError << ' ' << leftErrColumn->toDouble(ip) << std::endl; } leftErrColumn->fromDouble(ip, lError); } if (rError > rightErrColumn->toDouble(ip)) { if (g_log.is(Kernel::Logger::Priority::PRIO_DEBUG)) { g_log.debug() << " right for " << ip << ' ' << rError << ' ' << rightErrColumn->toDouble(ip) << std::endl; } rightErrColumn->fromDouble(ip, rError); } } // Output the quadratic estimate for comparrison. quadraticErrColumn->fromDouble(i, sqrt(V.get(i, i))); } }
void qFinderDMM::negativeLogDerivEvidenceLambdaPi(vector<double>& x, vector<double>& df){ try{ // cout << "\tstart negativeLogDerivEvidenceLambdaPi" << endl; vector<double> storeVector(numSamples, 0.0000); vector<double> derivative(numOTUs, 0.0000); vector<double> alpha(numOTUs, 0.0000); double store = 0.0000; double nu = 0.1000; double eta = 0.1000; double weight = 0.0000; for(int i=0;i<numSamples;i++){ weight += zMatrix[currentPartition][i]; } for(int i=0;i<numOTUs;i++){ if (m->control_pressed) { return; } // cout << "start i loop" << endl; // // cout << i << '\t' << alpha[i] << '\t' << x[i] << '\t' << exp(x[i]) << '\t' << store << endl; alpha[i] = exp(x[i]); store += alpha[i]; // cout << "before derivative" << endl; derivative[i] = weight * psi(alpha[i]); // cout << "after derivative" << endl; // cout << i << '\t' << alpha[i] << '\t' << psi(alpha[i]) << '\t' << derivative[i] << endl; for(int j=0;j<numSamples;j++){ double X = countMatrix[j][i]; double alphaX = X + alpha[i]; derivative[i] -= zMatrix[currentPartition][j] * psi(alphaX); storeVector[j] += alphaX; } // cout << "end i loop" << endl; } double sumStore = 0.0000; for(int i=0;i<numSamples;i++){ sumStore += zMatrix[currentPartition][i] * psi(storeVector[i]); } store = weight * psi(store); df.resize(numOTUs, 0.0000); for(int i=0;i<numOTUs;i++){ df[i] = alpha[i] * (nu + derivative[i] - store + sumStore) - eta; // cout << i << '\t' << df[i] << endl; } // cout << df.size() << endl; // cout << "\tend negativeLogDerivEvidenceLambdaPi" << endl; } catch(exception& e){ m->errorOut(e, "qFinderDMM", "negativeLogDerivEvidenceLambdaPi"); exit(1); } }
// メイン int main(int argc, char ** argv){ if(argc != 2){ std::cerr << "Usage: compute-border [CSVfile]" << std::endl; return -1; } // ファイルを読み込む。 // ファイルは、以下の構造をしているCSVである。 // // 1番目のデータ点のラベル,1番目のデータ点のxの値,1番目のデータ点のyの値,... // 2番目のデータ点のラベル,2番目のデータ点のxの値,2番目のデータ点のyの値,... // : // // 「ラベル」とは、ここでは分類区分の意味である。 // 境界線を生成した結果、なるべく各領域において同じラベルのデータ点だけが // 集まるようにしたい。 Eigen::MatrixXd data; if(!( numeric_csv_reader::read(data, argv[1]) )){ std::cerr << "Error found in the specified CSV file \"" << argv[1] << "\"." << std::endl; return -1; } // 読み込んだ内容を、ラベルと座標に分ける Eigen::VectorXd labels = data.block(0, 0, data.rows(), 1); Eigen::MatrixXd points = data.block(0, 1, data.rows(), data.cols() - 1); // データが何次元か(dim)と、データ数(num)を確認 int dim = points.cols(); int num = points.rows(); // ラベルは-1か1しか仮定していないので、それを確認 for(int i = 0; i < num; ++i){ if(labels(i) != 1.0 && labels(i) != -1.0){ std::cerr << "Label (first column) must be either 1 or -1." << std::endl; return -1; } } // 2次元の場合、 // ・点(p, q)のラベルが -1 (赤) と付いている場合、 // 分類成功となる条件は ap + bq - 1 > 0 であり、 // 最小化したい関数は (-(ap + bq - 1))+ と定める。 // ・点(p, q)のラベルが +1 (青) と付いている場合、 // 分類成功となる条件は ap + bq - 1 < 0 であり、 // 最小化したい関数は (ap + bq - 1)+ と定める。 // ただし、(z)+ = max{0, z} である(0を下回る場合は0にする)。 // // これらをまとめて、最小化したい関数を c(ap + bq - 1)+ と定める。 // ただし、cはラベルである。 // // この条件のもとで、関数値を最小化するような(a, b)を求める。 // 3次元以上の場合、変数の数が増えるだけで同様である。 // // さて、この関数をaについて最小化することを考える(bについても同様)。 // (z)+ は、一点を境に繋いだ半直線2本からなる。 // // \ // \ // \____ c(ap + bq - 1) (cp < 0) // →a // // / // / // ____/ c(ap + bq - 1) (cp > 0) // →a // // そのため、単に c(ap + bq - 1) = 0、すなわち a = (1 - bq)/p となる // 場所を選べばよい。 // なお実際には、「データ点すべてについてこの関数を足し合わせたもの」を // 最小化しないとならないので、各データ点(p, q)について (1 - bq)/p を // 求めておくことはもちろん、その (1 - bq)/p のうちどこが最小値を取るのかを // 計算できないとならない。 // 以下では、この (1 - bq)/p を「ブレークポイント」と呼び、全データ点に // 対するブレークポイントをソートして保持しておくものとする。 // // 最小値を求めるには、導関数を用いればよい。g(a) = c(ap + bq - 1) を微分すると // g'(a) = cp になるので、f(a) = (c(ap + bq - 1))+ の導関数は // ・もし (c(ap + bq - 1))+ = 0 ならば、f'(a) = 0 // ・もし (c(ap + bq - 1))+ > 0 ならば、f'(a) = cp // となる。 // ちなみに、導関数は以下のようになる。 // // [元の関数] // // \ cp < 0 cp > 0 / // \ / // \____ ____/ // →a →a // ↓導関数 ↓導関数 // //  ̄ ̄ ̄ cp // ──── 0 ──── 0 // ___ cp // // ところで、実際に求めたい導関数は、これを全データ点に対する導関数について // 加算したものである。つまり、 // ・cp < 0 となるブレークポイントがあったら、それ以下のaについて、 // 導関数の値をcpだけ加算する // ・cp > 0 となるブレークポイントがあったら、それ以上のaについて、 // 導関数の値をcpだけ加算する // とすれば導関数を求められる。 // ただ、これは不都合が大きい(ブレークポイントの先と後の両方に行き来して // 導関数の値を加算しないとならない)。そこで、必ず先だけを見ることのできる // 以下の方法を採用する。 // ただし、ブレークポイントbからb+1までの間の導関数を d[b] とかく。 // ・M = 0 // ・ブレークポイントの小さいものから順に以下のことを行う。 // ・cp < 0 となる場合、M += cp, d[b] = d[b-1] + |cp| (|・|は絶対値) // ・cp > 0 となる場合、d[b] = d[b-1] + cp // ・すべてのbについて、d[b] += M // // この導関数の値が0をまたいだ場所を検出すればよい。 // 最適化のために動かしたい変数(a, bに相当) Eigen::VectorXd coefs = Eigen::VectorXd::Zero(dim); // ブレークポイント // std::pair は「データ点の番号、ブレークポイントの値」の順 std::vector< std::pair<int, double> > breakpoints(num); // 導関数 std::vector< double > derivative(num); double derivative_base; int steps = 0; for(;;){ ++steps; std::cerr << "Computing step " << steps << "; "; display_classifier(coefs, dim, std::cerr); // coefsを更新した量のうち最大のもの // これが一定の値を下回ったら、計算を打ち切る double max_update = 0.0; // 各次元について、以下のことを行う for(int d = 0; d < dim; ++d){ // ブレークポイントを計算する。 // c(ap + bq - 1) = 0 (2次元)の場合は a = (1 - bq)/p とすればよいが // 一般の次元数の場合は、c(ap + bq + b'q' + b''q'' + ... - 1) = 0 つまり // a = (1 - bq - b'q' - b''q'' - ...)/p という計算になる。 for(int i = 0; i < num; ++i){ breakpoints[i].first = i; breakpoints[i].second = 1; for(int j = 0; j < dim; ++j){ if(j == d) continue; breakpoints[i].second -= coefs(j) * points(i, j); } breakpoints[i].second /= points(i, d); } // ブレークポイントをソートする std::sort(breakpoints.begin(), breakpoints.end(), [](const std::pair<int, double> & a, const std::pair<int, double> & b){ return(a.second < b.second); }); // ソートした順に、傾き(導関数)を求める derivative_base = 0.0; for(int i = 0; i < num; ++i){ // いま注目しているデータ点 int id = breakpoints[i].first; // 上記 cp の値を求める double deriv_single = labels(id) * points(id, d); // 導関数の値を加算する if(i == 0){ derivative[i] = std::fabs(deriv_single); }else{ derivative[i] = derivative[i-1] + std::fabs(deriv_single); } if(deriv_single < 0) derivative_base += deriv_single; } // 導関数が0を超えた(=最小値を取るときの)ブレークポイントを探す。 int min_breakpoint; for(min_breakpoint = 0; min_breakpoint < num; ++min_breakpoint){ if(derivative[min_breakpoint] + derivative_base >= 0.0) break; } if(min_breakpoint == num){ // これは起きないはずなのだが念のためチェック std::cerr << "Unexpected Error!" << std::endl; return -1; } // 係数(上記解説のaやb)を更新。見つかったブレークポイントの値にする double update = std::fabs(breakpoints[min_breakpoint].second - coefs(d)); coefs(d) = breakpoints[min_breakpoint].second; if(update > max_update) max_update = update; } if(max_update < EPSILON) break; } std::cout << "---------- Result of training ----------" << std::endl; std::cout << "Classify as class y = -1 if g(x_1, ..., x_" << dim << ") > 0," << std::endl; std::cout << "Classify as class y = +1 if g(x_1, ..., x_" << dim << ") < 0," << std::endl; std::cout << "(x_i: data point)" << std::endl; std::cout << "where" << std::endl; display_classifier(coefs, dim, std::cout); return 0; }
/*! Returns a tuple holding the permittivity and its derivative * \param[in] r evaluation point */ pcm::tuple<double, double> operator()(const double r) const { return pcm::make_tuple(value(r), derivative(r)); }
void Points::GenerateKernel(int L, int point_count, std::string title) { int g=point_count, m=0; LEGENDRE P_lm; LEGENDRE Y_P; LEGENDRE dP_lm; std::vector<std::vector<double> > d_kern(g); //kernel for derivative reconstruction std::vector<std::vector<double> > f_kern(g); //kernel for function (test) reconstruction std::vector<std::vector<double> > WT(g); std::complex<double> Y(0,0), Ylm(0,0), dYlm(0,0), Ymp1(0,0), ej(0,0), function(0,0), derivative(0,0); std::complex<double> im(0,1); double th1=0, ph1=0, sign=0; std::cout << title << std::endl; std::ofstream kernel(title); kernel.precision(15); for(int i=0; i<g; i++) { d_kern[i].resize(g); f_kern[i].resize(g); WT[i].resize(g); for(int j=0; j<g; j++) { for(double l=0; l<=L; l++) { for(double m_it=0; m_it<=(2*l); m_it++) { m=0; m = l-m_it; //std::cout << "m = " << m << ", l = " << l << std::endl; ej = m; sign=pow(-1.0,m_it); std::complex<double> exponential_prime(cos( Points::Phi[i]), (-1)*sin(Points::Phi[i])); std::complex<double> exponential(cos(m*Points::Phi[j]), sin(m*Points::Phi[j])); Ylm = P_lm.Yml(m, l, Points::Theta[i], Points::Phi[i]); Y = Y_P.Yml(m, l, Points::Theta[j], Points::Phi[j]); if( Theta[i] != 0 && ((m+1)<=l) ) { Ymp1 = m * (1.0/tan(Points::Theta[i])) * dP_lm.Yml(m, l, Points::Theta[i], Points::Phi[i]) + sqrt( (l-m)*(l+m+1) ) * exponential_prime * dP_lm.Yml(m+1, l, Points::Theta[i], Points::Phi[i]); } ///fill arrays with f=Y*Y for the function kernel and derivative kernel f_kern[i][j] += (conj(Y)*Ylm).real();//Y_real*Y_prime_real; d_kern[i][j] += (conj(Y)*Ymp1).real(); } } ///absorb weights into kernel WT[i][j] = Points::Weight[j]*4.0*PI; kernel << d_kern[i][j]*Points::Weight[j]*4.0*PI << " " << f_kern[i][j]*Points::Weight[j]*4.0*PI << " " << WT[i][j] << std::endl; } } kernel.close(); }
void Transform::derivative(Ensemble& iEnsemble) const { for(int i = 0; i < iEnsemble.size(); i++) { iEnsemble[i] = derivative(iEnsemble[i]); } }
bool distributor::distribute(MVar *arg0, MVar *arg1, MVar *arg2, MVar *arg3, std::string &func, int nr) { numCplx z; numReal d; bool res = false; if (nr == 1) { //if (charac->isZero()) { res = intDist(arg0,func); if (!res) { res = algCplxDist(arg0,func); } if (!res) { res = numRealDist(arg0,func); } if (!res) { res = numFuncDist(arg0,func); } if(arg0->getType()==DISTDAT) { DistDat dd; numReal nr; arg0->getDistDat(&dd); res = dd.prop(combiC,nr,func); if (res) { arg0->setNumReal(nr); } else { validC->setErr(ERRPROPDIST,dd.name); } } if (!res && func == "derivative") { Function f,g; if (arg0->getFunction(&f)){ if (derivative(g,f)) { arg0->setFunction(g); res = true; } else validC->setErr(NOTDIFFBAR); } } if (!res) { res = calcDist(arg0,func); } /*} else { if (!res && !charac->isZero()) { res = restDist(arg0,func); } }*/ if (!res) { res = matrixDist(arg0,func); } } else if (nr == 2) { //if (charac->isZero()) { res = twoIntDist(arg0,arg1,func); if (!res) { res = doubleIntDist(arg0,arg1,func); } if (!res) { res = tempDist(arg0,arg1,func); } if (!res) { res = doubleDoubleDist(arg0,arg1,func); } if (!res) { res = funcDoubleDist(arg0,arg1,func); } if (!res) { res = CplxCplxDist(arg0,arg1,func); } if (!res) { res = euklidDist(arg0,arg1,func); } if(arg0->getType()==DISTDAT) { DistDat dd; arg0->getDistDat(&dd); res = distFunc(dd,*arg1,func); *arg0 = *arg1; } /*} else { if (!res && (arg0->getType()==RESTPOLY || arg1->getType()==RESTPOLY)) { res = twoRestPolyDist(arg0,arg1,func); } if (!res) { res = restRestDist(arg0,arg1,func); } }*/ if (!res) { res = matrixDist(arg0,arg1,func); } if (!res) { res = matrixMatrixDist(arg0,arg1,func); } } else if (nr == 3) { /*MaceInt mi; if (func == "IF") { if (arg0->getInt(&mi)) { res = true; if (!mi.isZero()) *arg0 = *arg1; else *arg0 = *arg2; } else { validC->setErr(NOTBOOL,"IF"); } } if (!res) { res = dbldbldblDist(arg0,arg1,arg2,func); } if (!res) { res = matrixIntIntDist(arg0,arg1,arg2,func); } if (!res) { res = funcDblDblDist(arg0,arg1,arg2,func); }*/ } if (nr == 4) { /*res = matrixIntIntCplxDist(arg0,arg1,arg2,arg3,func); if (func == "spHarm") { if (*function) validC->setErr(CPLX,func); MaceInt mi0, mi1; numCplx resC; numReal nr0, nr1; double theta, phi; unsigned int m, n; if (arg0->getNumReal(&nr0) && arg1->getNumReal(&nr1)) { phi = nr0.get(); theta = nr1.get(); if (arg2->getInt(&mi0) && arg3->getInt(&mi1)) { clInt(mi0,m); clInt(mi1,n); resC.val.real() = boost::math::spherical_harmonic_r(n,m,theta,phi); resC.val.imag() = boost::math::spherical_harmonic_i(n,m,theta,phi); arg0->setNumCplx(resC); } else validC->setErr(NOTIR,"spHarm"); } else validC->setErr(NOTIR,"spHarm"); res = true; }*/ } if (!res) { combiCalc::dist dist; bool withInt; int vars; combiC.findDist(&withInt, func, dist, vars); if (dist != combiCalc::NONE && vars == nr) { DistDat dd; dd.name = func; if (distdist(arg0,arg1,arg2,dd,dist,nr,withInt)) { arg0->setDistDat(dd); } else { validC->setErr(ERRDIST,dd.name); } res = true; } } return res; }
/***********************************************************************//** * @brief Returns parameter gradient of model for a given event * * @param[in] model Model. * @param[in] event Event. * @param[in] ipar Parameter index for which gradient should be returned. * * This method uses a robust but dumb method to estimate parameter * gradients that have not been provided by the model. We use here a dumb * method as this method is likely used for spatial model parameters, and * the spatial model may eventually be noisy due to numerical integration * limits. * * The step size for the dumb method has been fixed to 0.0002, which * corresponds to abound 1 arcsec for parameters that are given in degrees. * The reasoning behind this value is that parameters that use numerical * gradients are typically angles, such as for example the position, and * we want to achieve arcsec precision with this method. * * @todo Implement a more precise numerical derivation scheme. This needs * a deep investigation as the scheme needs to be precise but also * robust (not sensitive to noise in the function). * @todo For the Riddler method, we simply remove any parameter boundaries * here for the computation to avoid any out of boundary errors. * We may have models, however, for which out of bound parameters lead * to illegal computations, such as division by zero or taking the * square root of negative values. * I cannot see any elegant method to catch this at this level. * Eventually, the higher level method should avoid going in a * parameter domain that is not defined. ***************************************************************************/ double GObservation::model_grad(const GModel& model, const GEvent& event, int ipar) const { // Initialise gradient double grad = 0.0; // Compute gradient only if parameter is free if (model[ipar].isfree()) { // If model has a gradient then use it if (model[ipar].hasgrad()) { grad = model[ipar].gradient(); } // ... otherwise compute it numerically else { // Get non-const model pointer (circumvent const correctness) GModel* ptr = (GModel*)&model; // Save current model parameter GModelPar current = (*ptr)[ipar]; // Get actual parameter value double x = model[ipar].value(); // Set fixed step size for computation of derivative. // By default, the step size is fixed to 0.0002, but if this would // violate a boundary, dx is reduced accordingly. In case that x // is right on the boundary, x is displaced slightly from the // boundary to allow evaluation of the derivative. #if !defined(G_GRAD_RIDDLER) const double step_size = 0.0002; double dx = step_size; if (model[ipar].hasmin()) { double dx_min = x - model[ipar].min(); if (dx_min == 0.0) { dx = step_size * x; if (dx == 0.0) { dx = step_size; } x += dx; } else if (dx_min < dx) { dx = dx_min; } } if (model[ipar].hasmax()) { double dx_max = model[ipar].max() - x; if (dx_max == 0.0) { dx = step_size * x; if (dx == 0.0) { dx = step_size; } x -= dx; } else if (dx_max < dx) { dx = dx_max; } } #endif // Remove any boundaries to avoid limitations (*ptr)[ipar].remove_range(); // Setup derivative function GObservation::model_func function(this, model, event, ipar); // Get derivative. We use a fixed step size here that has been // checked on spatial parameters of models GDerivative derivative(&function); #if defined(G_GRAD_RIDDLER) grad = derivative.value(x); #else grad = derivative.difference(x, dx); #endif // Restore current model parameter (*ptr)[ipar] = current; } // endelse: computed gradient numerically } // endif: model parameter was free // Return gradient return grad; }
/***********************************************************************//** * @brief Returns parameter gradient of Npred * * @param[in] model Gamma-ray source model. * @param[in] ipar Parameter index for which gradient should be returned. * * Computes * \f[\frac{{\rm d} N_{\rm pred}}{{\rm d} a_i}\f] * where * \f[N_{\rm pred} = \int_{\rm GTI} \int_{E_{\rm bounds}} \int_{\rm ROI} * S(\vec{p}, E, t) PSF(\vec{p'}, E', t' | \vec{d}, \vec{p}, E, t) \, * {\rm d}\vec{p'} {\rm d}E' {\rm d}t'\f] * and * \f$a_i\f$ is the model parameter \f$i\f$. * Furthermore, * \f$S(\vec{p}, E, t)\f$ is the source model, * \f$PSF(\vec{p'}, E', t' | \vec{d}, \vec{p}, E, t)\f$ is the point * spread function, * \f$\vec{p'}\f$ is the measured photon direction, * \f$E'\f$ is the measured photon energy, * \f$t'\f$ is the measured photon arrival time, * \f$\vec{p}\f$ is the true photon arrival direction, * \f$E\f$ is the true photon energy, * \f$t\f$ is the true photon arrival time, and * \f$d\f$ is the instrument pointing. * * This method uses a robust but dumb method to estimate gradients. This * method has turned out more robust then the Riddler's method implement * by the GDerivative::value() method. * * The step size for the dumb method has been fixed to 0.0002, which * corresponds to abound 1 arcsec for parameters that are given in degrees. * The reasoning behind this value is that parameters that use numerical * gradients are typically angles, such as for example the position, and * we want to achieve arcsec precision with this method. * * @todo Implement a more precise numerical derivation scheme. This needs * a deep investigation as the scheme needs to be precise but also * robust (not sensitive to noise in the function). * @todo For Riddler's method we simply remove any parameter boundaries here * for the computation to avoid any out of boundary errors. We may have * models, however, for which out of bound parameters lead to illegal * computations, such as division by zero or taking the square root of * negative values. * I cannot see any elegant method to catch this at this level. * Eventually, the higher level method should avoid going in a * parameter domain that is not defined. ***************************************************************************/ double GObservation::npred_grad(const GModel& model, int ipar) const { // Initialise result double grad = 0.0; // Compute gradient only if parameter is free if (model[ipar].isfree()) { // Get non-const model pointer (circumvent const correctness) GModel* ptr = const_cast<GModel*>(&model); // Save current model parameter GModelPar current = (*ptr)[ipar]; // Get actual parameter value double x = model[ipar].value(); // Determine fixed step size for computation of derivative. // By default, the step size is fixed to 0.0002, but if this would // violate a boundary, dx is reduced accordingly. In case that x // is right on the boundary, x is displaced slightly from the // boundary to allow evaluation of the derivative. #if !defined(G_GRAD_RIDDLER) const double step_size = 0.0002; double dx = step_size; if (model[ipar].hasmin()) { double dx_min = x - model[ipar].min(); if (dx_min == 0.0) { dx = step_size * x; if (dx == 0.0) { dx = step_size; } x += dx; } else if (dx_min < dx) { dx = dx_min; } } if (model[ipar].hasmax()) { double dx_max = model[ipar].max() - x; if (dx_max == 0.0) { dx = step_size * x; if (dx == 0.0) { dx = step_size; } x -= dx; } else if (dx_max < dx) { dx = dx_max; } } #endif // Remove any boundaries to avoid limitations (*ptr)[ipar].remove_range(); // Setup derivative function GObservation::npred_func function(this, model, ipar); // Get derivative. GDerivative derivative(&function); #if defined(G_GRAD_RIDDLER) grad = derivative.value(x); #else grad = derivative.difference(x, dx); #endif // Restore current model parameter (*ptr)[ipar] = current; } // endif: model parameter was free // Return result return grad; }
/** * \brief returns all the parameter values of A whose tangent is parallel to vector V. * \relates D2 */ std::vector<double> find_tangents_by_vector(Point V, D2<SBasis> const &A) { SBasis crs = dot(derivative(A), rot90(V)); return roots(crs); }
/** * \brief returns all the parameter values of A whose normal is parallel to vector V. * \relates D2 */ std::vector<double> find_normals_by_vector(Point V, D2<SBasis> const &A) { SBasis crs = dot(derivative(A), V); return roots(crs); }
/** * \brief returns all the parameter values of A whose normal passes through P. * \relates D2 */ std::vector<double> find_normals(Point P, D2<SBasis> const &A) { SBasis crs (dot(A - P, derivative(A))); return roots(crs); }
int main(int argc, char **argv) { int c = 0; double mass = SOLAR_MASS; double radius = SOLAR_RADIUS; double GRAVITATION_CONST = 6.67384E-8; double mu = 0.0; string n_output; while ((c = getopt(argc, argv, ":n:o:r:m:g:u:")) != -1) { switch (c) { case 'n': LANE_EMDEN_N = atof(optarg); break; case 'm': mass = atof(optarg); break; case 'r': radius = atof(optarg); break; case 'g': GRAVITATION_CONST = atof(optarg); break; case 'o': n_output = optarg; break; case 'u': mu = atof(optarg); break; } } // Generate basic values for the star. double rho_middle = 3.0 * mass / (4.0 * M_PI * pow(radius, 3.0)); double z_n = 0.0; double w_n = 0.0; double dwdz_n = 0.0; bool null_set = false; double A = 0.0; double K = 0.0; listDouble y(2), y_out(2), y_file(3); lane_emden_start(y); // Start values for ODE double h = 0.0001; double hx = 0.0001; double x = 0.0; int count = 0; vector< vector<double> > y_list; // Calculate the solutions for lane-emden. do { x += hx; listDouble dydx; derivative(x, y, dydx); integration_heun(y, dydx, x, h, y_out, derivative); y_file[0] = y_out[0]; y_file[1] = y_out[1]; y_file[2] = x; // A single entry of y_list is a vector with 0:w 1:dw/dz 2: z y_list.push_back(y_file); if (!null_set && y_out[0] <= 0.0) { derivative(x, y_out, dydx); z_n = x; w_n = y_out[0]; dwdz_n = y[1]; // Don't set the values again. null_set = true; } y = y_out; count++; } while (x < 20.0 && count < 1000000); // Calculate some needed values. double rho_core = rho_middle / (-3 * dwdz_n / z_n); A = z_n / radius; K = 4 * M_PI * GRAVITATION_CONST * pow(rho_core, (LANE_EMDEN_N - 1.0) / LANE_EMDEN_N) / ((LANE_EMDEN_N + 1.0) * pow(A, 2.0)); double mass_total = 4 * M_PI * rho_core * pow(radius, 3) * (- dwdz_n/z_n ); double masss_total_dimless = 4 * M_PI * rho_core * pow(z_n, 3) * (- dwdz_n/z_n ); double p_core = K * pow(rho_core, (LANE_EMDEN_N + 1) / LANE_EMDEN_N); double temp_core = p_core * mu / ( rho_core * GAS_CONST); // Write down the values. ofstream output_file; string output_filename = "lane_emden_"; if (n_output.empty()) { std::ostringstream sstream; sstream << LANE_EMDEN_N; std::string n = sstream.str(); n_output = n; } else { output_filename = output_filename.append(n_output).append(".dat"); } output_file.open(output_filename.c_str()); double w = 0.0; double dwdz = 0.0; double r = 0.0; double rho = 0.0; double z = 0.0; double p = 0.0; // Write down the lane-emden data. // The conten of lane_emden_n.dat is: // - z // - w // - dwdz // - r // - rho // - p for (unsigned int i = 0; i < y_list.size(); i++) { // Only calculate values until the edge of the star. if (z > z_n) { break; } w = y_list[i][0]; dwdz = y_list[i][1]; z = y_list[i][2]; r = z / A; rho = rho_core * pow(w, LANE_EMDEN_N); p = K * pow(rho, (LANE_EMDEN_N + 1) / LANE_EMDEN_N); // Write down the values. output_file << scientific << z << "\t" << w << "\t" << dwdz << "\t"; output_file << r << "\t" << rho << "\t" << p << endl; } output_file.close(); // Write down the constant solar data. // The content of solar_n.dat is: // - rho_crit // - rho_middle // - K // - A // - mass // - radius // - mass_total string output_solar_filename = "solar_"; output_solar_filename.append(n_output); output_solar_filename.append(".dat"); ofstream output_solar_file; output_solar_file.open(output_solar_filename.c_str()); output_solar_file << rho_core << "\t" << rho_middle << "\t" << K << "\t" << A << "\t" << mass << "\t" << radius << "\t" << z_n << "\t" << mass_total << "\t" << temp_core << "\t" << p_core << "\t" << masss_total_dimless << endl; output_file.close(); return 0; }
bool distributor::calcDist(MVar *arg, std::string &func) { differentialCalc::func FCTN = diffC.findVectorialFunc(func); if (FCTN != differentialCalc::NONE) { Function function; if (arg->getFunction(&function)) { MaceString MStr; std::vector<numReal> z; if (function.real()) { if (diffC.vectorialFunc(FCTN,function,z)) { std::complex<double> p; bool periodic = false; if (FCTN == differentialCalc::ZEROES) periodic = function.period(p); else if (FCTN == differentialCalc::EXTREMA) { Function der; derivative(der,function); periodic = der.period(p); } else if (FCTN == differentialCalc::INFLEXION) { Function der,der2; derivative(der,function); derivative(der2,der); periodic = der2.period(p); } periodic = periodic && p.imag() < 0.000001; for (int i = 0; i < z.size(); i++) { MStr.str = MStr.str.append(z[i].print(6)); if (periodic) { QString help; help = help.setNum(p.real()); MStr.str = MStr.str.append(" + k*"); MStr.str = MStr.str.append(help); } MStr.str = MStr.str.append(", "); } if (z.size() == 0) MStr.str = "No points found "; MStr.str.chop(2); } else { Function der; if (derivative(der,function)) MStr.str = "No points found."; else validC->setErr(NOTDIFFBAR); } arg->setString(MStr); return true; } else validC->setErr(NOTREALFUNC); } } else { FCTN = diffC.findCplxFunc(func); if (FCTN != differentialCalc::NONE) { std::complex<double> z; Function function; if (arg->getFunction(&function)) { if (diffC.cplxFunc(FCTN,function,z)) { numCplx nz; nz.val = z; arg->setNumCplx(nz); } else { MaceString MStr; MStr.str = "No period has been found."; arg->setString(MStr); } return true; } } } return false; }
int main(int argc, char** argv) { ros::init(argc, argv, "controller"); ros::NodeHandle nh; ros::NodeHandle nh_params("~"); ROS_INFO("running controller"); ros::Subscriber imu_sub = nh.subscribe("/firefly/imu", 1, &imuCallback); ros::Subscriber pose_sub = nh.subscribe("/firefly/fake_gps/pose", 1, &poseCallback); ros::Subscriber traj_sub = nh.subscribe("command/trajectory", 1, &MultiDofJointTrajectoryCallback); current_index = 0; double x_kp, x_ki, x_kd, y_kp, y_ki, y_kd, z_kp, z_ki, z_kd, yaw_kp, yaw_ki, yaw_kd, roll_limit, pitch_limit, yaw_limit, thrust_limit; nh_params.param("x_kp", x_kp, 1.0); nh_params.param("x_ki", x_ki, 0.0); nh_params.param("x_kd", x_kd, 0.0); nh_params.param("y_kp", y_kp, 1.0); nh_params.param("y_ki", y_ki, 0.0); nh_params.param("y_kd", y_kd, 0.0); nh_params.param("z_kp", z_kp, 1.0); nh_params.param("z_ki", z_ki, 0.0); nh_params.param("z_kd", z_kd, 0.0); nh_params.param("yaw_kp", yaw_kp, 1.0); nh_params.param("yaw_ki", yaw_ki, 0.0); nh_params.param("yaw_kd", yaw_kd, 0.0); nh_params.param("roll_limit", roll_limit, 0.0); nh_params.param("pitch_limit", pitch_limit, 0.0); nh_params.param("thrust_limit", thrust_limit, 0.0); nh_params.param("yaw_limit", yaw_limit, 0.0); // Chose one of the versions below. The first of these topic published determines the control mode. ros::Publisher command_pub = nh.advertise<mav_msgs::RollPitchYawrateThrust>("command/roll_pitch_yawrate_thrust", 1); // Start the dynamic_reconfigure server dynamic_reconfigure::Server<rotors_exercise::ControllerConfig> server; dynamic_reconfigure::Server<rotors_exercise::ControllerConfig>::CallbackType f; f = boost::bind(&reconfigure_callback, _1, _2); server.setCallback(f); ROS_INFO("Initializing controller ... "); /* Initialize here your controllers */ ros::Timer timer; timer = nh.createTimer(ros::Duration(0.2), timerCallback); //Timer for debugging // Run the control loop and Fly to x=0m y=0m z=1m ROS_INFO("Going to starting position [0,0,1] ..."); //positionLoop.setPoint(0.0, 0.0, 1.0, 0.0); setpoint_pos = tf::Vector3(0.,0.,1.); setpoint_yaw = 0.0; latest_pose_update_time = ros::Time::now(); while(ros::ok()) { ros::spinOnce(); delta_time_pose = (latest_pose.header.stamp - latest_pose_update_time).toSec() ; // Check if pose/imu/state data was received if ( (latest_pose.header.stamp.nsec > 0.0) && ((latest_pose.header.stamp - latest_pose_update_time).toSec() > 0.0) ) { latest_pose_update_time = latest_pose.header.stamp; //compute distance to next waypoint double distance = sqrt((setpoint_pos[0]-latest_pose.pose.position.x) * (setpoint_pos[0]-latest_pose.pose.position.x) + (setpoint_pos[1]-latest_pose.pose.position.y) * (setpoint_pos[1]-latest_pose.pose.position.y) + (setpoint_pos[2]-latest_pose.pose.position.z) * (setpoint_pos[2]-latest_pose.pose.position.z) ); if (distance < 0.5) { //there is still waypoints if (current_index < latest_trajectory.poses.size()) { ROS_INFO("Waypoint achieved! Moving to next waypoint"); geometry_msgs::PoseStamped wp; wp = latest_trajectory.poses[current_index]; setpoint_pos[0]=wp.pose.position.x; setpoint_pos[1]=wp.pose.position.y; setpoint_pos[2]=wp.pose.position.z; setpoint_yaw=tf::getYaw(wp.pose.orientation); current_index++; }else if (current_index == latest_trajectory.poses.size()) // print once waypoint achieved { ROS_INFO("Waypoint achieved! No more waypoints. Hovering"); current_index++; } } // run position loop double now = (double)ros::Time::now().toSec(); double delta_time = now - latest; latest = now; ROS_INFO_STREAM("1"); //Calculate error for each component latest_yaw = tf::getYaw(latest_pose.pose.orientation); ROS_INFO_STREAM("latest yaw: " << latest_yaw); ROS_INFO_STREAM("setpoint_pos: " << setpoint_pos); ROS_INFO_STREAM("latest_pose: " << latest_pose); computeError(setpoint_pos,latest_pose,setpoint_yaw, latest_yaw, error); //PROPORCIONAL Kp*error // DONE ON GET COMMANDS //INTEGRAL integral(error, delta_time, integral_limits, integral_error_accumulator); //DERIVATIVE derivative(error,delta_time, delta_error, previous_error); //Get commands x_vel_cmd = x_kp*error[0] + x_ki*integral_error_accumulator[0] + x_kd*delta_error[0]; y_vel_cmd = y_kp*error[1] + y_ki*integral_error_accumulator[1] + y_kd*delta_error[1]; z_vel_cmd = z_kp*error[2] + z_ki*integral_error_accumulator[2] + z_kd*delta_error[2]; yaw_rate = yaw_kp*error[3] + yaw_ki*integral_error_accumulator[3] + yaw_kd*delta_error[3]; // your desired velocities (or accelerations) should be stored in // x_vel_cmd, // y_vel_cmd, // z_vel_cmd, // yaw_rate wrap around!!!! //x_accel_cmd = x_vel_cmd // Map velocities (or accelerations) to angles roll, pitch roll_cmd = (x_vel_cmd*sin(latest_yaw) + y_vel_cmd*cos(latest_yaw))/GRAVETAT; pitch_cmd = (x_vel_cmd*cos(latest_yaw) + y_vel_cmd*sin(latest_yaw))/GRAVETAT; thrust = 1.5*GRAVETAT + 1.5*z_vel_cmd; //Rotate TF from WORLD TO BODY FRAME ------ARDRONE //tf::Vector3 vector3 (x_vel_cmd , y_vel_cmd, 0.0); //vector3 = rotateZ (vector3, -tf::getYaw(latest_pose.pose.orientation)); //pitch_cmd = vector3[0]; //roll_cmd = vector3[1]; //thrust = z_vel_cmd; // Saturate your request roll_cmd = (roll_cmd > roll_limit) ? roll_limit : ((roll_cmd < -roll_limit) ? -roll_limit : roll_cmd); pitch_cmd = (pitch_cmd > pitch_limit) ? pitch_limit : ((pitch_cmd < -pitch_limit)? -pitch_limit : pitch_cmd); thrust = (thrust > thrust_limit) ? thrust_limit: ((thrust < -thrust_limit) ? -thrust_limit: thrust); yaw_rate = (yaw_rate > yaw_limit) ? yaw_limit : ((yaw_rate < -yaw_limit) ? -yaw_limit : yaw_rate); // Send to the attitude controller: // roll angle [rad], pitch angle [rad], thrust [N][rad/s] mav_msgs::RollPitchYawrateThrust msg; msg.header = latest_pose.header; // use the latest information you have. msg.roll = -roll_cmd; msg.pitch = pitch_cmd; msg.thrust.z = thrust; msg.yaw_rate = yaw_rate; command_pub.publish(msg); } ros::Duration(control_loop_period/2.).sleep(); // may be set slower. } return 0; }
double PID::getPID(double error){ double value = proportional(error) + integral(error) + derivative(error); return trim(value); }
Real secondDerivative(Real x) const { return derivative(x)*interpolation_.derivative(x, true) + value(x)*interpolation_.secondDerivative(x, true); }
/* Compute satellite position & velocity at the given time * using this ephemeris. * * @throw InvalidRequest if required data has not been stored. */ Xvt GloEphemeris::svXvt(const CommonTime& epoch) const throw( gpstk::InvalidRequest ) { // Check that the given epoch is within the available time limits. // We have to add a margin of 15 minutes (900 seconds). if ( epoch < (ephTime - 900.0) || epoch >= (ephTime + 900.0) ) { InvalidRequest e( "Requested time is out of ephemeris data" ); GPSTK_THROW(e); } // Values to be returned will be stored here Xvt sv; // If the exact epoch is found, let's return the values if ( epoch == ephTime ) // exact match for epoch { sv.x[0] = x[0]*1.e3; // m sv.x[1] = x[1]*1.e3; // m sv.x[2] = x[2]*1.e3; // m sv.v[0] = v[0]*1.e3; // m/sec sv.v[1] = v[1]*1.e3; // m/sec sv.v[2] = v[2]*1.e3; // m/sec // In the GLONASS system, 'clkbias' already includes the // relativistic correction, therefore we must substract the late // from the former. sv.relcorr = sv.computeRelativityCorrection(); sv.clkbias = clkbias + clkdrift * (epoch - ephTime) - sv.relcorr; sv.clkdrift = clkdrift; sv.frame = ReferenceFrame::PZ90; // We are done, let's return return sv; } // Get the data out of the GloRecord structure double px( x[0] ); // X coordinate (km) double vx( v[0] ); // X velocity (km/s) double ax( a[0] ); // X acceleration (km/s^2) double py( x[1] ); // Y coordinate double vy( v[1] ); // Y velocity double ay( a[1] ); // Y acceleration double pz( x[2] ); // Z coordinate double vz( v[2] ); // Z velocity double az( a[2] ); // Z acceleration // We will need some PZ-90 ellipsoid parameters PZ90Ellipsoid pz90; double we( pz90.angVelocity() ); // Get sidereal time at Greenwich at 0 hours UT double gst( getSidTime( ephTime ) ); double s0( gst*PI/12.0 ); YDSTime ytime( ephTime ); double numSeconds( ytime.sod ); double s( s0 + we*numSeconds ); double cs( std::cos(s) ); double ss( std::sin(s) ); // Initial state matrix Vector<double> initialState(6), accel(3), dxt1(6), dxt2(6), dxt3(6), dxt4(6), tempRes(6); // Get the reference state out of GloEphemeris object data. Values // must be rotated from PZ-90 to an absolute coordinate system // Initial x coordinate (m) initialState(0) = (px*cs - py*ss); // Initial y coordinate initialState(2) = (px*ss + py*cs); // Initial z coordinate initialState(4) = pz; // Initial x velocity (m/s) initialState(1) = (vx*cs - vy*ss - we*initialState(2) ); // Initial y velocity initialState(3) = (vx*ss + vy*cs + we*initialState(0) ); // Initial z velocity initialState(5) = vz; // Integrate satellite state to desired epoch using the given step double rkStep( step ); if ( (epoch - ephTime) < 0.0 ) rkStep = step*(-1.0); CommonTime workEpoch( ephTime ); double tolerance( 1e-9 ); bool done( false ); while (!done) { // If we are about to overstep, change the stepsize appropriately // to hit our target final time. if( rkStep > 0.0 ) { if( (workEpoch + rkStep) > epoch ) rkStep = (epoch - workEpoch); } else { if ( (workEpoch + rkStep) < epoch ) rkStep = (epoch - workEpoch); } numSeconds += rkStep; s = s0 + we*( numSeconds ); cs = std::cos(s); ss = std::sin(s); // Accelerations are computed once per iteration accel(0) = ax*cs - ay*ss; accel(1) = ax*ss + ay*cs; accel(2) = az; dxt1 = derivative( initialState, accel ); for( int j = 0; j < 6; ++j ) tempRes(j) = initialState(j) + rkStep*dxt1(j)/2.0; dxt2 = derivative( tempRes, accel ); for( int j = 0; j < 6; ++j ) tempRes(j) = initialState(j) + rkStep*dxt2(j)/2.0; dxt3 = derivative( tempRes, accel ); for( int j = 0; j < 6; ++j ) tempRes(j) = initialState(j) + rkStep*dxt3(j); dxt4 = derivative( tempRes, accel ); for( int j = 0; j < 6; ++j ) initialState(j) = initialState(j) + rkStep * ( dxt1(j) + 2.0 * ( dxt2(j) + dxt3(j) ) + dxt4(j) ) / 6.0; // If we are within tolerance of the target time, we are done. workEpoch += rkStep; if ( std::fabs(epoch - workEpoch ) < tolerance ) done = true; } // End of 'while (!done)...' px = initialState(0); py = initialState(2); pz = initialState(4); vx = initialState(1); vy = initialState(3); vz = initialState(5); sv.x[0] = 1000.0*( px*cs + py*ss ); // X coordinate sv.x[1] = 1000.0*(-px*ss + py*cs); // Y coordinate sv.x[2] = 1000.0*pz; // Z coordinate sv.v[0] = 1000.0*( vx*cs + vy*ss + we*(sv.x[1]/1000.0) ); // X velocity sv.v[1] = 1000.0*(-vx*ss + vy*cs - we*(sv.x[0]/1000.0) ); // Y velocity sv.v[2] = 1000.0*vz; // Z velocity // In the GLONASS system, 'clkbias' already includes the relativistic // correction, therefore we must substract the late from the former. sv.relcorr = sv.computeRelativityCorrection(); sv.clkbias = clkbias + clkdrift * (epoch - ephTime) - sv.relcorr; sv.clkdrift = clkdrift; sv.frame = ReferenceFrame::PZ90; // We are done, let's return return sv; } // End of method 'GloEphemeris::svXvt(const CommonTime& t)'
int main() { //Two booleans are used when taking calculating the runtime int clockOn = 0; int totalClockOn = 1; //Variables used in calculating runtime clock_t start, clockBeforeRead, clockAfterRead, clockAfterLowPass, clockAfterHighPass, clockAfterDer, clockAfterSquare, clockAfterMWI, clockAfterIdentifyPeaks, end; double cpuTimeUsed; double timeReadData = 0; double timeLowPass = 0; double timeHighPass = 0; double timeDerivative = 0; double timeSquare = 0; double timeMWindowInt = 0; double timeIdentifyPeaks = 0; //The file to read from static const char filename[] = "ECG.txt"; FILE *file = fopen(filename, "r"); //Arrays and a variable to keep track of the values after each filter int input[13] = { 0 }; int afterLowpass[33] = { 0 }; int afterHighpass[5] = { 0 }; int afterDerivative = 0; int afterSquare[30] = { 0 }; int afterWindowIntegration[3] = { 0 }; int value; int counter = 0; if (totalClockOn) { start = clock(); } //The loops runs as long as it has not reached the end of the file while (!feof(file)) { if (clockOn) { clockBeforeRead = clock(); } counter++; //The nex value is saved in the variable "value" value = getNextData(file); insertArray(input, 13, value); /*if (clockOn) { clockAfterRead = clock(); timeReadData =+ ((double) ((clockAfterRead - clockBeforeRead)) / CLOCKS_PER_SEC ) * 1000; }*/ lowPass(input, afterLowpass); /*if (clockOn) { clockAfterLowPass = clock(); timeLowPass =+ ((double) ((clockAfterLowPass - clockAfterRead)) / CLOCKS_PER_SEC ) * 1000; }*/ highPass(afterLowpass, afterHighpass); /*if (clockOn) { clockAfterHighPass = clock(); timeHighPass =+ ((double) ((clockAfterHighPass - clockAfterLowPass)) / CLOCKS_PER_SEC ) * 1000; }*/ afterDerivative = derivative(afterHighpass); /*if (clockOn) { clockAfterDer = clock(); timeDerivative =+ ((double) ((clockAfterDer - clockAfterHighPass)) / CLOCKS_PER_SEC ) * 1000; }*/ square(afterDerivative, afterSquare); /*if (clockOn) { clockAfterSquare = clock(); timeSquare =+ ((double) ((clockAfterSquare - clockAfterDer)) / CLOCKS_PER_SEC ) * 1000; }*/ mWindowIntegration(afterSquare, afterWindowIntegration); if (clockOn) { clockAfterMWI = clock(); timeMWindowInt = +((double) ((clockAfterMWI - clockBeforeRead)) / CLOCKS_PER_SEC) * 1000; } identifyPeaks(afterWindowIntegration); if (clockOn) { clockAfterIdentifyPeaks = clock(); timeIdentifyPeaks = +((double) ((clockAfterIdentifyPeaks - clockAfterMWI)) / CLOCKS_PER_SEC) * 1000; } } if (totalClockOn) { end = clock(); cpuTimeUsed = ((double) ((end - start)) / CLOCKS_PER_SEC) * 1000; printf("Total cpu time: %f milliseconds\n", cpuTimeUsed); } if (clockOn) { //printf("Average time read data: %f nanoseconds\nAverage time Low Pass filter: %f nanoseconds\nAverage time High Pass filter: %f nanoseconds\n", timeReadData, timeLowPass, timeHighPass); //printf("Average time derivative filter: %f nanoseconds\nAverage time square filter: %f nanoseconds\nAverage time moving window integration filter: %f nanoseconds\n",timeDerivative, timeSquare, timeMWindowInt); printf("Time to apply filters: %f milliseconds\n", timeMWindowInt); printf("Time to identify peaks: %f milliseconds\n", timeIdentifyPeaks); } return 0; }
const std::vector<T> secondDerivative(const std::vector<T> & signal, const BorderInterpolation interpolation = CONTINUED) { const unsigned size = signal.size(); const unsigned n = signal.size() - 1; std::vector<T> derivative(size); if (size == 0) { return derivative; } else if (size == 1) { derivative.push_back(T(0)); return derivative; } else { // compute boundaries // left boundary // derivative[0] = signal[-1] - 2 * signal[0] + signal[1] switch(interpolation) { case CONTINUED: // signal[-1] := signal[0] derivative[0] = -signal[0] + signal[1]; break; case CIRCULAR: // signal[-1] := signal[n] derivative[0] = signal[n] - 2 * signal[0] + signal[1]; break; case INTERPOLATED: // signal[-1] := signal[0] + (signal[0] - signal[1]) = 2 * signal[0] - signal[1] derivative[0] = 0; break; case REFLECTED: // signal[-1] := signal[0] derivative[0] = -signal[0] + signal[1]; break; case ZERO: // signal[-1] := 0 derivative[0] = -2 * signal[0] + signal[1]; break; default: ErrorObj error("Unknown interpolation method."); error.setFunctionName("Differentiate"); error.setErrorCode(INVALID_ARGUMENT); throw error; } // right boundary // derivative[n] = signal[n-1] - 2 * signal[n] + signal[n+1] switch(interpolation) { case CONTINUED: // signal[n+1] := signal[n] derivative[n] = signal[n-1] -signal[n]; break; case CIRCULAR: // signal[n+1] := signal[0] derivative[n] = signal[n-1] - 2 * signal[n] + signal[0]; break; case INTERPOLATED: // signal[n+1] := signal[n] + (signal[n] - signal[n-1]) = 2 * signal[n] - signal[n-1] derivative[n] = 0; break; case REFLECTED: // signal[n+1] := signal[n] derivative[n] = signal[n-1] -signal[n]; break; case ZERO: // signal[n+1] := 0 derivative[n] = signal[n-1] - 2 * signal[n]; break; default: ErrorObj error("Unknown interpolation method."); error.setFunctionName("Differentiate"); error.setErrorCode(INVALID_ARGUMENT); throw error; } // compute inner part for (unsigned i = 1; i < n; ++i) { derivative[i] = signal[i-1] - 2 * signal[i] + signal[i+1]; } return derivative; } }
void eval_derivative(void) { int i, n; // evaluate 1st arg to get function F p1 = cdr(p1); push(car(p1)); eval(); // evaluate 2nd arg and then... // example result of 2nd arg what to do // // d(f) nil guess X, N = nil // d(f,2) 2 guess X, N = 2 // d(f,x) x X = x, N = nil // d(f,x,2) x X = x, N = 2 // d(f,x,y) x X = x, N = y p1 = cdr(p1); push(car(p1)); eval(); p2 = pop(); if (p2 == symbol(NIL)) { guess(); push(symbol(NIL)); } else if (isnum(p2)) { guess(); push(p2); } else { push(p2); p1 = cdr(p1); push(car(p1)); eval(); } N = pop(); X = pop(); F = pop(); while (1) { // N might be a symbol instead of a number if (isnum(N)) { push(N); n = pop_integer(); if (n == (int) 0x80000000) stop("nth derivative: check n"); } else n = 1; push(F); if (n >= 0) { for (i = 0; i < n; i++) { push(X); derivative(); } } else { n = -n; for (i = 0; i < n; i++) { push(X); integral(); } } F = pop(); // if N is nil then arglist is exhausted if (N == symbol(NIL)) break; // otherwise... // N arg1 what to do // // number nil break // number number N = arg1, continue // number symbol X = arg1, N = arg2, continue // // symbol nil X = N, N = nil, continue // symbol number X = N, N = arg1, continue // symbol symbol X = N, N = arg1, continue if (isnum(N)) { p1 = cdr(p1); push(car(p1)); eval(); N = pop(); if (N == symbol(NIL)) break; // arglist exhausted if (isnum(N)) ; // N = arg1 else { X = N; // X = arg1 p1 = cdr(p1); push(car(p1)); eval(); N = pop(); // N = arg2 } } else { X = N; // X = N p1 = cdr(p1); push(car(p1)); eval(); N = pop(); // N = arg1 } } push(F); // final result }
void neuron :: train (double learning_factor, non_linear_function noLin) { double * wp = weights; double * vp = inputs; double delta = error * learning_factor; for (int ind = 0; ind < synapses; ind++) {* (wp++) += * (vp++) * delta * derivative (output, noLin);} }
inline void derivative(Matrix<DType>* matrix, Matrix<DType>* alpha) { derivative(matrix, matrix, alpha); }
///Evaluates the gradient at the Point2F coordinate Vector2F BaseExpression::gradient(Point2F& pPoint) { return Vector2F( derivative(XVAR)->evaluate(pPoint), derivative(YVAR)->evaluate(pPoint)); }