diff --git a/src/Trim.cpp b/src/Trim.cpp
index 73065a7c9583fa19ed48ea7c6aa2930c362801f2..8b2d544d303306b27c7db8a4de04bf8d31f87a42 100644
--- a/src/Trim.cpp
+++ b/src/Trim.cpp
@@ -195,6 +195,7 @@ int main (int argc, char const* argv[])
solver = new FGNelderMead(trimmer,initialGuess,
lowerBound, upperBound, initialStepSize,iterMax,rtol,
abstol,speed,showConvergeStatus,showSimplex,pause,&callback);
+ while(solver->status()) solver->update();
}
catch (const std::runtime_error & e)
{
@@ -231,6 +232,8 @@ int main (int argc, char const* argv[])
ss.x.add(new FGStateSpace::Alpha);
ss.x.add(new FGStateSpace::Theta);
ss.x.add(new FGStateSpace::Q);
+ ss.x.add(new FGStateSpace::Theta);
+ ss.x.add(new FGStateSpace::Q);
if (thruster0->GetType()==FGThruster::ttPropeller)
{
diff --git a/src/initialization/FGTrimmer.cpp b/src/initialization/FGTrimmer.cpp
index ed46681379739043b1294c5c1c13e8a28cfd746b..164597aace423764ec1d309c3152b41d51bafb86 100644
--- a/src/initialization/FGTrimmer.cpp
+++ b/src/initialization/FGTrimmer.cpp
@@ -36,7 +36,7 @@ FGTrimmer::FGTrimmer(FGFDMExec & fdm, Constraints & constraints) :
m_fdm.Setdt(1./120.);
}
-void FGTrimmer::constrain(const std::vector<double> & v)
+std::vector<double> FGTrimmer::constrain(const std::vector<double> & v)
{
// unpack design vector
double throttle = v[0];
@@ -153,6 +153,10 @@ void FGTrimmer::constrain(const std::vector<double> & v)
<< "\tr\trad/s\t" << r
<< std::fixed
<< std::endl;*/
+ std::vector<double> data;
+ data.push_back(phi);
+ data.push_back(theta);
+ return data;
}
void FGTrimmer::getSolution(const std::vector<double> & v, std::vector<double> & x, std::vector<double> & u)
@@ -344,8 +348,7 @@ double FGTrimmer::eval(const std::vector<double> & v)
double cost = 0;
for (int iter=0;;iter++)
{
-
- constrain(v);
+ constrain(v);
dvt = (propagate()->GetUVW(1)*propagate()->GetUVWdot(1) +
propagate()->GetUVW(2)*propagate()->GetUVWdot(2) +
propagate()->GetUVW(3)*propagate()->GetUVWdot(3))/
diff --git a/src/initialization/FGTrimmer.h b/src/initialization/FGTrimmer.h
index 31433b92f1f1e88562bcf90728346cef17671c77..dfce1ed1c6c4720a81a622dbcdd18cce7bfede17 100644
--- a/src/initialization/FGTrimmer.h
+++ b/src/initialization/FGTrimmer.h
@@ -41,7 +41,7 @@ public:
bool coordinatedTurn, stabAxisRoll;
};
FGTrimmer(FGFDMExec & fdm, Constraints & constraints);
- void constrain(const vector<double> & v);
+ std::vector<double> constrain(const vector<double> & v);
void getSolution(const vector<double> & v, vector<double> & x, vector<double> & u);
void printSolution(const vector<double> & v);
void printState();
diff --git a/src/math/FGNelderMead.cpp b/src/math/FGNelderMead.cpp
index 9917ee89b285b9e0699ccc835c7d0420e47f4b83..5bcd27eeb0880843ae911383c9541f971fed34d4 100644
--- a/src/math/FGNelderMead.cpp
+++ b/src/math/FGNelderMead.cpp
@@ -37,204 +37,211 @@ FGNelderMead::FGNelderMead(Function & f, const std::vector<double> & initialGues
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_showSimplex(showSimplex), m_callback(callback), m_stopRequested(false)
+ m_showSimplex(showSimplex), m_callback(callback), m_status(1),
+ initialGuess(initialGuess), initialStepSize(initialStepSize),
+ iterMax(iterMax), rtol(rtol), abstol(abstol),
+ speed(speed), showConvergeStatus(showConvergeStatus), showSimplex(showSimplex),
+ pause(pause), rtolI(), minCostPrevResize(), minCost(), minCostPrev(), maxCost(),
+ nextMaxCost(), iter()
{
- // setup
- std::cout.precision(3);
- double rtolI = 0;
- double minCostPrevResize = 0, minCost = 0;
- double minCostPrev = 0, maxCost = 0, nextMaxCost = 0;
- int iter = 0;
+}
- // solve simplex
- while (!m_stopRequested)
- {
- // reinitialize simplex whenever rtol condition is met
- if ( rtolI < rtol || iter == 0)
- {
- std::vector<double> guess(m_nDim);
- if (iter == 0)
- {
- //std::cout << "constructing simplex" << std::endl;
- guess = initialGuess;
- }
- else
- {
- if (std::abs(minCost-minCostPrevResize) < 1e-20)
- {
- std::cout << "\nunable to escape local minimum" << std::endl;
- break;
- }
- //std::cout << "reinitializing step size" << std::endl;
- guess = m_simplex[m_iMin];
- minCostPrevResize = minCost;
- }
- constructSimplex(guess,initialStepSize);
- }
+void FGNelderMead::update()
+{
+ std::cout.precision(3);
- // find vertex costs
- for (int vertex=0;vertex<m_nVert;vertex++)
- {
- try
- {
- m_cost[vertex] = m_f.eval(m_simplex[vertex]);
- }
- catch (const std::exception & e)
+ // reinitialize simplex whenever rtol condition is met
+ if ( rtolI < rtol || iter == 0)
+ {
+ std::vector<double> guess(m_nDim);
+ if (iter == 0)
+ {
+ //std::cout << "constructing simplex" << std::endl;
+ guess = initialGuess;
+ }
+ else
+ {
+ if (std::abs(minCost-minCostPrevResize) < 1e-20)
{
- throw;
+ std::cout << "\nunable to escape local minimum" << std::endl;
+ m_status = -1;
return;
}
- if (m_callback) m_callback->eval(m_simplex[vertex]);
- }
+ //std::cout << "reinitializing step size" << std::endl;
+ guess = m_simplex[m_iMin];
+ minCostPrevResize = minCost;
+ }
+ constructSimplex(guess,initialStepSize);
+ }
- // find max cost, next max cost, and min cost
- m_iMax = m_iNextMax = m_iMin = 0;
- for (int vertex=0;vertex<m_nVert;vertex++)
- {
- if ( m_cost[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;
+ // find vertex costs
+ for (int vertex=0;vertex<m_nVert;vertex++)
+ {
+ try
+ {
+ m_cost[vertex] = m_f.eval(m_simplex[vertex]);
+ }
+ catch (const std::exception & e)
+ {
+ m_status = -1;
+ throw;
+ return;
+ }
+ if (m_callback) m_callback->eval(m_simplex[vertex]);
+ }
- }
+ // find max cost, next max cost, and min cost
+ m_iMax = m_iNextMax = m_iMin = 0;
+ for (int vertex=0;vertex<m_nVert;vertex++)
+ {
+ if ( m_cost[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;
- // 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<double>::epsilon()));
+ }
- // check for max iteratin break condition
- if (iter > iterMax)
- {
- std::cout << "\nmax iterations exceeded" << std::endl;
- break;
+ // 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<double>::epsilon()));
- }
- // check for convergence break condition
- else if ( m_cost[m_iMin] < abstol )
- {
- std::cout << "\nsimplex converged" << std::endl;
- break;
- }
+ // check for max iteratin break condition
+ if (iter > iterMax)
+ {
+ std::cout << "\nmax iterations exceeded" << std::endl;
+ m_status = -1;
+ 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<m_nDim;dim++)
- {
- m_elemSum[dim] = 0;
- for (int vertex=0;vertex<m_nVert;vertex++)
- m_elemSum[dim] += m_simplex[vertex][dim];
- }
+ // compute element sum of simplex vertices
+ for (int dim=0;dim<m_nDim;dim++)
+ {
+ m_elemSum[dim] = 0;
+ for (int vertex=0;vertex<m_nVert;vertex++)
+ m_elemSum[dim] += m_simplex[vertex][dim];
+ }
- // min and max costs
- minCostPrev = minCost;
- minCost = m_cost[m_iMin];
- maxCost = m_cost[m_iMax];
- nextMaxCost = m_cost[m_iNextMax];
+ // min and max costs
+ minCostPrev = minCost;
+ minCost = m_cost[m_iMin];
+ maxCost = m_cost[m_iMax];
+ nextMaxCost = m_cost[m_iNextMax];
- // output cost and simplex
- if (showConvergeStatus)
- {
- if ( (minCostPrev + std::numeric_limits<float>::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 << "\ti: " << 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<m_nVert;j++)
- std::cout << "\t" << std::scientific
- << std::setw(10) << m_cost[j];
- std::cout << std::endl;
- for (int j=0;j<m_nVert;j++) std::cout << "\t\t" << j;
- std::cout << std::endl;
- for (int i=0;i<m_nDim;i++)
- {
- for (int j=0;j<m_nVert;j++)
- std::cout << "\t" << std::setw(10) << m_simplex[j][i];
- std::cout << std::endl;
- }
- std::cout << std::fixed
- << "\n\tiMax: " << m_iMax
- << "\t\tiNextMax: " << m_iNextMax
- << "\t\tiMin: " << m_iMin << std::endl;
- }
+ // output cost and simplex
+ if (showConvergeStatus)
+ {
+ if ( (minCostPrev + std::numeric_limits<float>::epsilon() )
+ < minCost && minCostPrev != 0)
+ {
+ std::cout << "\twarning: simplex cost increased"
+ << std::scientific
+ << "\n\tcost: " << minCost
+ << "\n\tcost previous: " << minCostPrev
+ << std::fixed << std::endl;
+ }
- if (pause)
- {
- std::cout << "paused, press any key to continue" << std::endl;
- std::cin.get();
- }
+ std::cout << "\ti: " << 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<m_nVert;j++)
+ std::cout << "\t" << std::scientific
+ << std::setw(10) << m_cost[j];
+ std::cout << std::endl;
+ for (int j=0;j<m_nVert;j++) std::cout << "\t\t" << j;
+ std::cout << std::endl;
+ for (int i=0;i<m_nDim;i++)
+ {
+ for (int j=0;j<m_nVert;j++)
+ std::cout << "\t" << std::setw(10) << m_simplex[j][i];
+ std::cout << std::endl;
+ }
+ std::cout << std::fixed
+ << "\n\tiMax: " << m_iMax
+ << "\t\tiNextMax: " << m_iNextMax
+ << "\t\tiMin: " << m_iMin << std::endl;
+ }
+ if (pause)
+ {
+ std::cout << "paused, press any key to continue" << std::endl;
+ std::cin.get();
+ }
- // costs
-
- try
+
+ // costs
+
+ try
+ {
+ // try inversion
+ double costTry = tryStretch(-1.0);
+
+ // if lower cost than best, then try further stretch by speed factor
+ if (costTry < minCost)
{
- // try inversion
- double costTry = tryStretch(-1.0);
+ costTry = tryStretch(speed);
+ }
+ // otherwise try a contraction
+ else if (costTry > nextMaxCost)
+ {
+ // 1d contraction
+ costTry = tryStretch(1./speed);
- // if lower cost than best, then try further stretch by speed factor
- if (costTry < minCost)
- {
- costTry = tryStretch(speed);
- }
- // otherwise try a contraction
- else if (costTry > nextMaxCost)
+ // if greater than max cost, contract about min
+ if (costTry > maxCost)
{
- // 1d contraction
- costTry = tryStretch(1./speed);
-
- // if greater than max cost, contract about min
- if (costTry > maxCost)
- {
- if (showSimplex)
- std::cout << "multiD contraction about: " << m_iMin << std::endl;
- contract();
- }
+ if (showSimplex)
+ std::cout << "multiD contraction about: " << m_iMin << std::endl;
+ contract();
}
}
+ }
- catch (const std::exception & e)
- {
- throw;
- stop();
- }
+ catch (const std::exception & e)
+ {
+ throw;
+ m_status = -1;
+ return;
+ }
+
+ // iteration
+ iter++;
- // iteration
- iter++;
- }
- std::cout << "\ti\t: " << iter << std::endl;
- std::cout << std::scientific;
- std::cout << "\trtol\t: " << rtolI << std::endl;
- std::cout << "\tcost\t: " << m_cost[m_iMin] << std::endl;
- std::cout << std::fixed;
}
-void FGNelderMead::stop()
+int FGNelderMead::status()
{
- std::cout << "FGNelderMead: stop requested." << std::endl;
- m_stopRequested = true;
+ if (m_status == 0)
+ {
+ std::cout << "\ti\t: " << iter << std::endl;
+ std::cout << std::scientific;
+ std::cout << "\trtol\t: " << rtolI << std::endl;
+ std::cout << "\tcost\t: " << m_cost[m_iMin] << std::endl;
+ std::cout << std::fixed;
+ }
+ return m_status;
}
std::vector<double> FGNelderMead::getSolution()
diff --git a/src/math/FGNelderMead.h b/src/math/FGNelderMead.h
index 56395aea4925f7d5f72dfcdc2a057e5cd0404713..4dee0bfde8f1e82ba216cca2d9fa8960b339f039 100644
--- a/src/math/FGNelderMead.h
+++ b/src/math/FGNelderMead.h
@@ -53,7 +53,8 @@ public:
Callback * callback=NULL);
std::vector<double> getSolution();
- void stop();
+ void update();
+ int status();
private:
// attributes
@@ -66,7 +67,14 @@ private:
std::vector<double> m_cost;
std::vector<double> m_elemSum;
bool m_showSimplex;
- bool m_stopRequested;
+ bool m_status;
+ const std::vector<double> & initialGuess;
+ const std::vector<double> & initialStepSize;
+ int iterMax, iter;
+ double rtol,abstol,speed;
+ bool showConvergeStatus, showSimplex, pause;
+ double rtolI, minCostPrevResize, minCost, minCostPrev,
+ maxCost, nextMaxCost;
// methods
double tryStretch(double factor);