Removed while loop from FGNelderMead solver.

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)
     {

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))/

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

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()

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