/* * FGNelderMead.cpp * Copyright (C) James Goppert 2010 * * FGNelderMead.cpp is free software: you can redistribute it and/or modify it * under the terms of the GNU Lesser General Public License as published by the * Free Software Foundation, either version 2 of the License, or * (at your option) any later version. * * FGNelderMead.cpp is distributed in the hope that it will be useful, but * WITHOUT ANY WARRANTY; without even the implied warranty of * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. * See the GNU Lesser General Public License for more details. * * You should have received a copy of the GNU Lesser General Public License along * with this program. If not, see . */ #include "FGNelderMead.h" #include #include #include #include #include #include #include #include namespace JSBSim { FGNelderMead::FGNelderMead(Function * f, const std::vector & initialGuess, const std::vector & lowerBound, const std::vector & upperBound, const std::vector & initialStepSize, int iterMax, double rtol, double abstol, double speed, double randomization, bool showConvergeStatus, bool showSimplex, bool pause, Callback * callback) : m_f(f), m_callback(callback), m_randomization(randomization), m_lowerBound(lowerBound), m_upperBound(upperBound), m_nDim(initialGuess.size()), m_nVert(m_nDim+1), m_iMax(1), m_iNextMax(1), m_iMin(1), m_simplex(m_nVert), m_cost(m_nVert), m_elemSum(m_nDim), m_status(1), initialGuess(initialGuess), initialStepSize(initialStepSize), iterMax(iterMax), iter(), rtol(rtol), abstol(abstol), speed(speed), showConvergeStatus(showConvergeStatus), showSimplex(showSimplex), pause(pause), rtolI(), minCostPrevResize(1), minCost(), minCostPrev(), maxCost(), nextMaxCost() { srand ( time(NULL) ); // seed random number generator } void FGNelderMead::update() { std::cout.precision(3); // reinitialize simplex whenever rtol condition is met if ( rtolI < rtol || iter == 0) { std::vector guess(m_nDim); if (iter == 0) { //std::cout << "constructing simplex" << std::endl; guess = initialGuess; } else { if (std::abs(minCost-minCostPrevResize) < std::numeric_limits::epsilon()) { throw std::runtime_error("unable to escape local minimum!"); m_status = -1; return; } //std::cout << "reinitializing step size" << std::endl; guess = m_simplex[m_iMin]; minCostPrevResize = minCost; } constructSimplex(guess,initialStepSize); } // find vertex costs for (int vertex=0;vertex m_cost[m_iMax] ) { m_iMax = vertex; } else if ( m_cost[vertex] > m_cost[m_iNextMax] || m_iMax == m_iNextMax ) m_iNextMax = vertex; else if ( m_cost[vertex] < m_cost[m_iMin] ) m_iMin = vertex; } // callback if (m_callback) m_callback->eval(m_simplex[m_iMin]); // compute relative tolerance rtolI = 2*std::abs(m_cost[m_iMax] - m_cost[m_iMin])/(std::abs(m_cost[m_iMax]+std::abs(m_cost[m_iMin])+ std::numeric_limits::epsilon())); // check for max iteration break condition if (iter > iterMax) { m_status = -1; throw std::runtime_error("max iterations exceeded!"); return; } // check for convergence break condition else if ( m_cost[m_iMin] < abstol ) { //std::cout << "\nsimplex converged" << std::endl; m_status = 0; return; } // compute element sum of simplex vertices for (int dim=0;dim::epsilon() ) < minCost && minCostPrev != 0) { std::cout << "\twarning: simplex cost increased" << std::scientific << "\n\tcost: " << minCost << "\n\tcost previous: " << minCostPrev << std::fixed << std::endl; } std::cout << "i: " << iter << std::scientific << "\tcost: " << m_cost[m_iMin] << "\trtol: " << rtolI << std::fixed << "\talpha: " << m_simplex[m_iMin][2]*180/M_PI << "\tbeta: " << m_simplex[m_iMin][5]*180/M_PI << "\tthrottle: " << m_simplex[m_iMin][0] << "\televator: " << m_simplex[m_iMin][1] << "\taileron: " << m_simplex[m_iMin][3] << "\trudder: " << m_simplex[m_iMin][4] << std::endl; } if (showSimplex) { std::cout << "simplex: " << std::endl;; for (int j=0;j nextMaxCost) { // 1d contraction costTry = tryStretch(1./speed); //std::cout << "cost Try 2: " << costTry << std::endl; // if greater than max cost, contract about min if (costTry > maxCost) { if (showSimplex) std::cout << "multiD contraction about: " << m_iMin << std::endl; contract(); } else { if (showSimplex) std::cout << "contraction about: " << m_iMin << std::endl; } } } catch (const std::exception & e) { throw; m_status = -1; return; } // iteration iter++; } int FGNelderMead::status() { return m_status; } double FGNelderMead::getRandomFactor() { double randFact = 1+(float(rand() % 1000)/500-1)*m_randomization; //std::cout << "random factor: " << randFact << std::endl;; return randFact; } std::vector FGNelderMead::getSolution() { return m_simplex[m_iMin]; } double FGNelderMead::tryStretch(double factor) { // randomize factor so we can avoid locking situations factor = factor*getRandomFactor(); // create trial vertex double a= (1.0-factor)/m_nDim; double b = a - factor; std::vector tryVertex(m_nDim); for (int dim=0;dim & guess, const std::vector & stepSize) { for (int vertex=0;vertex & vertex, const std::vector & lowerBound, const std::vector & upperBound) { for (int dim=0;dim upperBound[dim]) vertex[dim] = upperBound[dim]; else if (vertex[dim] < lowerBound[dim]) vertex[dim] = lowerBound[dim]; } } double FGNelderMead::eval(const std::vector & vertex, bool check) { if (check) { double cost0 = m_f->eval(vertex); double cost1 = m_f->eval(vertex); if ((cost0 - cost1) > std::numeric_limits::epsilon()) { std::stringstream msg; msg.precision(10); msg << std::scientific << "dynamics not stable!" << "\tdiff: " << cost1 - cost0 << "\tcost0: " << cost0 << "\tcost1: " << cost1 << std::endl; std::cout << msg.str(); //throw std::runtime_error(msg.str()); } else { return cost1; } } else { return m_f->eval(vertex); } } } // JSBSim // vim:ts=4:sw=4