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));
}
