JSBSim Flight Dynamics Model  1.0 (02 March 2017)
An Open Source Flight Dynamics and Control Software Library in C++
FGNelderMead.cpp
1 /*
2  * FGNelderMead.cpp
3  * Copyright (C) James Goppert 2010 <james.goppert@gmail.com>
4  *
5  * FGNelderMead.cpp is free software: you can redistribute it and/or modify it
6  * under the terms of the GNU Lesser General Public License as published by the
7  * Free Software Foundation, either version 2 of the License, or
8  * (at your option) any later version.
9  *
10  * FGNelderMead.cpp is distributed in the hope that it will be useful, but
11  * WITHOUT ANY WARRANTY; without even the implied warranty of
12  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
13  * See the GNU Lesser General Public License for more details.
14  *
15  * You should have received a copy of the GNU Lesser General Public License along
16  * with this program. If not, see <http://www.gnu.org/licenses/>.
17  */
18 
19 #include "FGNelderMead.h"
20 #include <limits>
21 #include <cmath>
22 #include <cstdlib>
23 #include <iomanip>
24 #include <iostream>
25 #include <sstream>
26 #include <stdexcept>
27 #include <ctime>
28 
29 namespace JSBSim
30 {
31 
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),
44  m_status(1),
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(),
49  nextMaxCost()
50 {
51  srand ( time(NULL) ); // seed random number generator
52 }
53 
54 void FGNelderMead::update()
55 {
56  std::cout.precision(3);
57 
58  // reinitialize simplex whenever rtol condition is met
59  if ( rtolI < rtol || iter == 0)
60  {
61  std::vector<double> guess(m_nDim);
62  if (iter == 0)
63  {
64  //std::cout << "constructing simplex" << std::endl;
65  guess = initialGuess;
66  }
67  else
68  {
69  if (std::abs(minCost-minCostPrevResize) < std::numeric_limits<float>::epsilon())
70  {
71  m_status = -1;
72  throw std::runtime_error("unable to escape local minimum!");
73  }
74  //std::cout << "reinitializing step size" << std::endl;
75  guess = m_simplex[m_iMin];
76  minCostPrevResize = minCost;
77  }
78  constructSimplex(guess,initialStepSize);
79  }
80 
81  // find vertex costs
82  for (unsigned int vertex=0;vertex<m_nVert;vertex++)
83  {
84  try
85  {
86  m_cost[vertex] = eval(m_simplex[vertex]);
87  }
88  catch (...)
89  {
90  m_status = -1;
91  throw;
92  }
93  }
94 
95  // find max cost, next max cost, and min cost
96  m_iMax = m_iNextMax = m_iMin = 0;
97  for (unsigned int vertex=0;vertex<m_nVert;vertex++)
98  {
99  if ( m_cost[vertex] > m_cost[m_iMax] )
100  {
101  m_iMax = vertex;
102  }
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;
105 
106  }
107 
108  // callback
109  if (m_callback) m_callback->eval(m_simplex[m_iMin]);
110 
111  // compute relative tolerance
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()));
115 
116  // check for max iteration break condition
117  if (iter > iterMax)
118  {
119  m_status = -1;
120  throw std::runtime_error("max iterations exceeded!");
121  }
122  // check for convergence break condition
123  else if ( m_cost[m_iMin] < abstol )
124  {
125  //std::cout << "\nsimplex converged" << std::endl;
126  m_status = 0;
127  return;
128  }
129 
130  // compute element sum of simplex vertices
131  for (unsigned int dim=0;dim<m_nDim;dim++)
132  {
133  m_elemSum[dim] = 0;
134  for (unsigned int vertex=0;vertex<m_nVert;vertex++)
135  m_elemSum[dim] += m_simplex[vertex][dim];
136  }
137 
138  // min and max costs
139  minCostPrev = minCost;
140  minCost = m_cost[m_iMin];
141  maxCost = m_cost[m_iMax];
142  nextMaxCost = m_cost[m_iNextMax];
143 
144  // output cost and simplex
145  if (showConvergeStatus)
146  {
147  if ( (minCostPrev + std::numeric_limits<float>::epsilon() )
148  < minCost && minCostPrev != 0)
149  {
150  std::cout << "\twarning: simplex cost increased"
151  << std::scientific
152  << "\n\tcost: " << minCost
153  << "\n\tcost previous: " << minCostPrev
154  << std::fixed << std::endl;
155  }
156 
157  std::cout << "i: " << iter
158  << std::scientific
159  << "\tcost: " << m_cost[m_iMin]
160  << "\trtol: " << rtolI
161  << std::fixed
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]
168  << std::endl;
169  }
170  if (showSimplex)
171  {
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++)
180  {
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;
184  }
185  std::cout << std::fixed
186  << "\n\tiMax: " << m_iMax
187  << "\t\tiNextMax: " << m_iNextMax
188  << "\t\tiMin: " << m_iMin << std::endl;
189  }
190 
191  if (pause)
192  {
193  std::cout << "paused, press any key to continue" << std::endl;
194  std::cin.get();
195  }
196 
197 
198  // costs
199 
200  try
201  {
202  // try inversion
203  double costTry = tryStretch(-1.0);
204  //std::cout << "cost Try 0: " << costTry << std::endl;
205 
206  // if lower cost than best, then try further stretch by double speed factor
207  if (costTry < minCost)
208  {
209  double costTry0 = costTry;
210  costTry = tryStretch(speed);
211  //std::cout << "cost Try 1: " << costTry << std::endl;
212 
213  if (showSimplex)
214  {
215  if (costTry < costTry0) std::cout << "inversion about: " << m_iMax << std::endl;
216  else std::cout << "inversion and stretch about: " << m_iMax << std::endl;
217  }
218  }
219  // otherwise try a contraction
220  else if (costTry > nextMaxCost)
221  {
222  // 1d contraction
223  costTry = tryStretch(1./speed);
224  //std::cout << "cost Try 2: " << costTry << std::endl;
225 
226  // if greater than max cost, contract about min
227  if (costTry > maxCost)
228  {
229  if (showSimplex)
230  std::cout << "multiD contraction about: " << m_iMin << std::endl;
231  contract();
232  }
233  else
234  {
235  if (showSimplex)
236  std::cout << "contraction about: " << m_iMin << std::endl;
237  }
238  }
239  }
240 
241  catch (...)
242  {
243  m_status = -1;
244  throw;
245  }
246 
247  // iteration
248  iter++;
249 
250 }
251 
252 int FGNelderMead::status()
253 {
254  return m_status;
255 }
256 
257 double FGNelderMead::getRandomFactor()
258 {
259  double randFact = 1+(float(rand() % 1000)/500-1)*m_randomization;
260  //std::cout << "random factor: " << randFact << std::endl;;
261  return randFact;
262 }
263 
264 std::vector<double> FGNelderMead::getSolution()
265 {
266  return m_simplex[m_iMin];
267 }
268 
269 double FGNelderMead::tryStretch(double factor)
270 {
271  // randomize factor so we can avoid locking situations
272  factor = factor*getRandomFactor();
273 
274  // create trial vertex
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++)
279  {
280  tryVertex[dim] = m_elemSum[dim]*a - m_simplex[m_iMax][dim]*b;
281  boundVertex(tryVertex,m_lowerBound,m_upperBound);
282  }
283 
284  // find trial cost
285  double costTry = eval(tryVertex);
286 
287  // if trial cost lower than max
288  if (costTry < m_cost[m_iMax])
289  {
290  // update the element sum of the simplex
291  for (unsigned int dim=0;dim<m_nDim;dim++) m_elemSum[dim] +=
292  tryVertex[dim] - m_simplex[m_iMax][dim];
293  // replace the max vertex with the trial vertex
294  for (unsigned int dim=0;dim<m_nDim;dim++) m_simplex[m_iMax][dim] = tryVertex[dim];
295  // update the cost
296  m_cost[m_iMax] = costTry;
297  if (showSimplex) std::cout << "stretched\t" << m_iMax << "\tby : " << factor << std::endl;
298  }
299  return costTry;
300 }
301 
302 void FGNelderMead::contract()
303 {
304  for (unsigned int dim=0;dim<m_nDim;dim++)
305  {
306  for (unsigned int vertex=0;vertex<m_nVert;vertex++)
307  {
308  m_simplex[vertex][dim] =
309  getRandomFactor()*0.5*(m_simplex[vertex][dim] +
310  m_simplex[m_iMin][dim]);
311  }
312  }
313 }
314 
315 void FGNelderMead::constructSimplex(const std::vector<double> & guess,
316  const std::vector<double> & stepSize)
317 {
318  for (unsigned int vertex=0;vertex<m_nVert;vertex++)
319  {
320  m_simplex[vertex] = guess;
321  }
322 
323  for (unsigned int dim=0;dim<m_nDim;dim++)
324  {
325  int vertex = dim + 1;
326  m_simplex[vertex][dim] += stepSize[dim]*getRandomFactor();
327  boundVertex(m_simplex[vertex],m_lowerBound,m_upperBound);
328  }
329  if (showSimplex)
330  {
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++)
335  {
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;
339  }
340  }
341 }
342 
343 void FGNelderMead::boundVertex(std::vector<double> & vertex,
344  const std::vector<double> & lowerBound,
345  const std::vector<double> & upperBound)
346 {
347  for (unsigned int dim=0;dim<m_nDim;dim++)
348  {
349  if (vertex[dim] > upperBound[dim]) vertex[dim] = upperBound[dim];
350  else if (vertex[dim] < lowerBound[dim]) vertex[dim] = lowerBound[dim];
351  }
352 }
353 
354 double FGNelderMead::eval(const std::vector<double> & vertex, bool check)
355 {
356  if (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;
361  msg.precision(10);
362  msg << std::scientific
363  << "dynamics not stable!"
364  << "\tdiff: " << cost1 - cost0
365  << "\tcost0: " << cost0
366  << "\tcost1: " << cost1
367  << std::endl;
368  std::cout << msg.str();
369  //throw std::runtime_error(msg.str());
370  } else {
371  return cost1;
372  }
373  }
374  return m_f->eval(vertex);
375 }
376 
377 } // JSBSim
378 
379 
380 // vim:ts=4:sw=4