19 #include "FGNelderMead.h" 32 FGNelderMead::FGNelderMead(Function * f,
const std::vector<double> & initialGuess,
33 const std::vector<double> & lowerBound,
34 const std::vector<double> & upperBound,
35 const std::vector<double> & initialStepSize,
int iterMax,
36 double rtol,
double abstol,
double speed,
double randomization,
37 bool showConvergeStatus,
38 bool showSimplex,
bool pause, Callback * callback) :
39 m_f(f), m_callback(callback), m_randomization(randomization),
40 m_lowerBound(lowerBound), m_upperBound(upperBound),
41 m_nDim(initialGuess.size()), m_nVert(m_nDim+1),
42 m_iMax(1), m_iNextMax(1), m_iMin(1),
43 m_simplex(m_nVert), m_cost(m_nVert), m_elemSum(m_nDim),
45 initialGuess(initialGuess), initialStepSize(initialStepSize),
46 iterMax(iterMax), iter(), rtol(rtol), abstol(abstol),
47 speed(speed), showConvergeStatus(showConvergeStatus), showSimplex(showSimplex),
48 pause(pause), rtolI(), minCostPrevResize(1), minCost(), minCostPrev(), maxCost(),
54 void FGNelderMead::update()
56 std::cout.precision(3);
59 if ( rtolI < rtol || iter == 0)
61 std::vector<double> guess(m_nDim);
69 if (std::abs(minCost-minCostPrevResize) < std::numeric_limits<float>::epsilon())
72 throw std::runtime_error(
"unable to escape local minimum!");
75 guess = m_simplex[m_iMin];
76 minCostPrevResize = minCost;
78 constructSimplex(guess,initialStepSize);
82 for (
unsigned int vertex=0;vertex<m_nVert;vertex++)
86 m_cost[vertex] = eval(m_simplex[vertex]);
96 m_iMax = m_iNextMax = m_iMin = 0;
97 for (
unsigned int vertex=0;vertex<m_nVert;vertex++)
99 if ( m_cost[vertex] > m_cost[m_iMax] )
103 else if ( m_cost[vertex] > m_cost[m_iNextMax] || m_iMax == m_iNextMax ) m_iNextMax = vertex;
104 else if ( m_cost[vertex] < m_cost[m_iMin] ) m_iMin = vertex;
109 if (m_callback) m_callback->eval(m_simplex[m_iMin]);
112 rtolI = 2*std::abs(m_cost[m_iMax] -
113 m_cost[m_iMin])/(std::abs(m_cost[m_iMax]+std::abs(m_cost[m_iMin])+
114 std::numeric_limits<double>::epsilon()));
120 throw std::runtime_error(
"max iterations exceeded!");
123 else if ( m_cost[m_iMin] < abstol )
131 for (
unsigned int dim=0;dim<m_nDim;dim++)
134 for (
unsigned int vertex=0;vertex<m_nVert;vertex++)
135 m_elemSum[dim] += m_simplex[vertex][dim];
139 minCostPrev = minCost;
140 minCost = m_cost[m_iMin];
141 maxCost = m_cost[m_iMax];
142 nextMaxCost = m_cost[m_iNextMax];
145 if (showConvergeStatus)
147 if ( (minCostPrev + std::numeric_limits<float>::epsilon() )
148 < minCost && minCostPrev != 0)
150 std::cout <<
"\twarning: simplex cost increased" 152 <<
"\n\tcost: " << minCost
153 <<
"\n\tcost previous: " << minCostPrev
154 << std::fixed << std::endl;
157 std::cout <<
"i: " << iter
159 <<
"\tcost: " << m_cost[m_iMin]
160 <<
"\trtol: " << rtolI
162 <<
"\talpha: " << m_simplex[m_iMin][2]*180/M_PI
163 <<
"\tbeta: " << m_simplex[m_iMin][5]*180/M_PI
164 <<
"\tthrottle: " << m_simplex[m_iMin][0]
165 <<
"\televator: " << m_simplex[m_iMin][1]
166 <<
"\taileron: " << m_simplex[m_iMin][3]
167 <<
"\trudder: " << m_simplex[m_iMin][4]
172 std::cout <<
"simplex: " << std::endl;;
173 for (
unsigned int j=0;j<m_nVert;j++)
174 std::cout <<
"\t" << std::scientific
175 << std::setw(10) << m_cost[j];
176 std::cout << std::endl;
177 for (
unsigned int j=0;j<m_nVert;j++) std::cout <<
"\t\t" << j;
178 std::cout << std::endl;
179 for (
unsigned int i=0;i<m_nDim;i++)
181 for (
unsigned int j=0;j<m_nVert;j++)
182 std::cout <<
"\t" << std::setw(10) << m_simplex[j][i];
183 std::cout << std::endl;
185 std::cout << std::fixed
186 <<
"\n\tiMax: " << m_iMax
187 <<
"\t\tiNextMax: " << m_iNextMax
188 <<
"\t\tiMin: " << m_iMin << std::endl;
193 std::cout <<
"paused, press any key to continue" << std::endl;
203 double costTry = tryStretch(-1.0);
207 if (costTry < minCost)
209 double costTry0 = costTry;
210 costTry = tryStretch(speed);
215 if (costTry < costTry0) std::cout <<
"inversion about: " << m_iMax << std::endl;
216 else std::cout <<
"inversion and stretch about: " << m_iMax << std::endl;
220 else if (costTry > nextMaxCost)
223 costTry = tryStretch(1./speed);
227 if (costTry > maxCost)
230 std::cout <<
"multiD contraction about: " << m_iMin << std::endl;
236 std::cout <<
"contraction about: " << m_iMin << std::endl;
252 int FGNelderMead::status()
257 double FGNelderMead::getRandomFactor()
259 double randFact = 1+(float(rand() % 1000)/500-1)*m_randomization;
264 std::vector<double> FGNelderMead::getSolution()
266 return m_simplex[m_iMin];
269 double FGNelderMead::tryStretch(
double factor)
272 factor = factor*getRandomFactor();
275 double a= (1.0-factor)/m_nDim;
276 double b = a - factor;
277 std::vector<double> tryVertex(m_nDim);
278 for (
unsigned int dim=0;dim<m_nDim;dim++)
280 tryVertex[dim] = m_elemSum[dim]*a - m_simplex[m_iMax][dim]*b;
281 boundVertex(tryVertex,m_lowerBound,m_upperBound);
285 double costTry = eval(tryVertex);
288 if (costTry < m_cost[m_iMax])
291 for (
unsigned int dim=0;dim<m_nDim;dim++) m_elemSum[dim] +=
292 tryVertex[dim] - m_simplex[m_iMax][dim];
294 for (
unsigned int dim=0;dim<m_nDim;dim++) m_simplex[m_iMax][dim] = tryVertex[dim];
296 m_cost[m_iMax] = costTry;
297 if (showSimplex) std::cout <<
"stretched\t" << m_iMax <<
"\tby : " << factor << std::endl;
302 void FGNelderMead::contract()
304 for (
unsigned int dim=0;dim<m_nDim;dim++)
306 for (
unsigned int vertex=0;vertex<m_nVert;vertex++)
308 m_simplex[vertex][dim] =
309 getRandomFactor()*0.5*(m_simplex[vertex][dim] +
310 m_simplex[m_iMin][dim]);
315 void FGNelderMead::constructSimplex(
const std::vector<double> & guess,
316 const std::vector<double> & stepSize)
318 for (
unsigned int vertex=0;vertex<m_nVert;vertex++)
320 m_simplex[vertex] = guess;
323 for (
unsigned int dim=0;dim<m_nDim;dim++)
325 int vertex = dim + 1;
326 m_simplex[vertex][dim] += stepSize[dim]*getRandomFactor();
327 boundVertex(m_simplex[vertex],m_lowerBound,m_upperBound);
331 std::cout <<
"simplex: " << std::endl;;
332 for (
unsigned int j=0;j<m_nVert;j++) std::cout <<
"\t\t" << j;
333 std::cout << std::endl;
334 for (
unsigned int i=0;i<m_nDim;i++)
336 for (
unsigned int j=0;j<m_nVert;j++)
337 std::cout <<
"\t" << std::setw(10) << m_simplex[j][i];
338 std::cout << std::endl;
343 void FGNelderMead::boundVertex(std::vector<double> & vertex,
344 const std::vector<double> & lowerBound,
345 const std::vector<double> & upperBound)
347 for (
unsigned int dim=0;dim<m_nDim;dim++)
349 if (vertex[dim] > upperBound[dim]) vertex[dim] = upperBound[dim];
350 else if (vertex[dim] < lowerBound[dim]) vertex[dim] = lowerBound[dim];
354 double FGNelderMead::eval(
const std::vector<double> & vertex,
bool check)
357 double cost0 = m_f->eval(vertex);
358 double cost1 = m_f->eval(vertex);
359 if ((cost0 - cost1) > std::numeric_limits<float>::epsilon()) {
360 std::stringstream msg;
362 msg << std::scientific
363 <<
"dynamics not stable!" 364 <<
"\tdiff: " << cost1 - cost0
365 <<
"\tcost0: " << cost0
366 <<
"\tcost1: " << cost1
368 std::cout << msg.str();
374 return m_f->eval(vertex);