FGSimplexTrim::FGSimplexTrim(FGFDMExec * fdm, TrimMode mode)
{
std::clock_t time_start=clock(), time_trimDone;
// variables
FGTrimmer::Constraints constraints;
if (fdm->GetDebugLevel() > 0) {
std::cout << "\n-----Performing Simplex Based Trim --------------\n" << std::endl;
}
// defaults
std::string aircraftName = fdm->GetAircraft()->GetAircraftName();
FGPropertyNode* node = fdm->GetPropertyManager()->GetNode();
double rtol = node->GetDouble("trim/solver/rtol");
double abstol = node->GetDouble("trim/solver/abstol");
double speed = node->GetDouble("trim/solver/speed"); // must be > 1, 2 typical
double random = node->GetDouble("trim/solver/random");
int iterMax = int(node->GetDouble("trim/solver/iterMax"));
bool showConvergence = node->GetBool("trim/solver/showConvergence");
bool pause = node->GetBool("trim/solver/pause");
bool showSimplex = node->GetBool("trim/solver/showSimplex");
// flight conditions
double phi = fdm->GetIC()->GetPhiRadIC();
double theta = fdm->GetIC()->GetThetaRadIC();
double gd = fdm->GetInertial()->gravity();
constraints.velocity = fdm->GetIC()->GetVtrueFpsIC();
constraints.altitude = fdm->GetIC()->GetAltitudeASLFtIC();
constraints.gamma = fdm->GetIC()->GetFlightPathAngleRadIC();
constraints.rollRate = 0;
constraints.pitchRate = 0;
constraints.yawRate = tan(phi)*gd*cos(theta)/constraints.velocity;
constraints.stabAxisRoll = true; // FIXME, make this an option
// initial solver state
int n = 6;
std::vector<double> initialGuess(n), lowerBound(n), upperBound(n), initialStepSize(n);
lowerBound[0] = node->GetDouble("trim/solver/throttleMin");
lowerBound[1] = node->GetDouble("trim/solver/elevatorMin");
lowerBound[2] = node->GetDouble("trim/solver/alphaMin");
lowerBound[3] = node->GetDouble("trim/solver/aileronMin");
lowerBound[4] = node->GetDouble("trim/solver/rudderMin");
lowerBound[5] = node->GetDouble("trim/solver/betaMin");
upperBound[0] = node->GetDouble("trim/solver/throttleMax");
upperBound[1] = node->GetDouble("trim/solver/elevatorMax");
upperBound[2] = node->GetDouble("trim/solver/alphaMax");
upperBound[3] = node->GetDouble("trim/solver/aileronMax");
upperBound[4] = node->GetDouble("trim/solver/rudderMax");
upperBound[5] = node->GetDouble("trim/solver/betaMax");
initialStepSize[0] = node->GetDouble("trim/solver/throttleStep");
initialStepSize[1] = node->GetDouble("trim/solver/elevatorStep");
initialStepSize[2] = node->GetDouble("trim/solver/alphaStep");
initialStepSize[3] = node->GetDouble("trim/solver/aileronStep");
initialStepSize[4] = node->GetDouble("trim/solver/rudderStep");
initialStepSize[5] = node->GetDouble("trim/solver/betaStep");
initialGuess[0] = node->GetDouble("trim/solver/throttleGuess");
initialGuess[1] = node->GetDouble("trim/solver/elevatorGuess");
initialGuess[2] = node->GetDouble("trim/solver/alphaGuess");
initialGuess[3] = node->GetDouble("trim/solver/aileronGuess");
initialGuess[4] = node->GetDouble("trim/solver/rudderGuess");
initialGuess[5] = node->GetDouble("trim/solver/betaGuess");
// solve
FGTrimmer * trimmer = new FGTrimmer(fdm, &constraints);
Callback callback(aircraftName, trimmer);
FGNelderMead * solver = NULL;
solver = new FGNelderMead(trimmer,initialGuess,
lowerBound, upperBound, initialStepSize,iterMax,rtol,
abstol,speed,random,showConvergence,showSimplex,pause,&callback);
while(solver->status()==1) solver->update();
time_trimDone = std::clock();
// output
if (fdm->GetDebugLevel() > 0) {
trimmer->printSolution(std::cout,solver->getSolution());
std::cout << "\nfinal cost: " << std::scientific << std::setw(10) << trimmer->eval(solver->getSolution()) << std::endl;
std::cout << "\ntrim computation time: " << (time_trimDone - time_start)/double(CLOCKS_PER_SEC) << "s \n" << std::endl;
}
delete solver;
delete trimmer;
}
FGSimplexTrim::FGSimplexTrim(FGFDMExec * fdmPtr, TrimMode mode)
{
using namespace JSBSim;
std::clock_t time_start=clock(), time_trimDone, time_linDone;
// variables
FGFDMExec & fdm = *fdmPtr;
fdm.Setdt(1./120);
FGTrimmer::Constraints constraints;
std::cout << "\n-----Performaing Simplex Based Trim --------------\n" << std::endl;
// defaults
constraints.velocity = fdm.GetAuxiliary()->GetVt();
constraints.altitude = fdm.GetPropagate()->GetAltitudeASL();
std::string aircraft = fdm.GetAircraft()->GetAircraftName();
double rtol = 10*std::numeric_limits<float>::epsilon();
double abstol = 10*std::numeric_limits<double>::epsilon();
double speed = 1.8; // must be > 1, 2 typical
double random = 0.0; // random scale factor added to all simplex calcs
int iterMax = 2000;
bool showConvergeStatus = false;
bool pause = false;
bool showSimplex = false;
bool variablePropPitch = false;
int debugLevel = 0;
std::string fileName = aircraft;
// input
//std::cout << "input ( press enter to accept [default] )\n" << std::endl;
// load model
std::string aircraftName = fdm.GetAircraft()->GetAircraftName();
//prompt("\tdebug level\t\t",debugLevel);
//fdm.SetDebugLevel(debugLevel);
//std::cout << "model selection" << std::endl;
//while (1)
//{
//prompt("\taircraft\t\t",aircraft);
//prompt("\toutput file name\t",fileName);
//fdm.LoadModel("../aircraft","../engine","../systems",aircraft);
//aircraftName = fdm.GetAircraft()->GetAircraftName();
//if (aircraftName == "")
//{
//std::cout << "\tfailed to load aircraft" << std::endl;
//}
//else
//{
//std::cout << "\tsuccessfully loaded: " << aircraftName << std::endl;
//break;
//}
//}
// Turn on propulsion system
fdm.GetPropulsion()->InitRunning(-1);
// get propulsion pointer to determine type/ etc.
FGEngine * engine0 = fdm.GetPropulsion()->GetEngine(0);
FGThruster * thruster0 = engine0->GetThruster();
// flight conditions
//std::cout << "\nflight conditions: " << std::endl;
//prompt("\taltitude, ft\t\t",constraints.altitude);
//prompt("\tvelocity, ft/s\t\t",constraints.velocity);
//prompt("\tgamma, deg\t\t",constraints.gamma); constraints.gamma = constraints.gamma*M_PI/180;
double phi = fdm.GetPropagate()->GetEuler(1);
double theta = fdm.GetPropagate()->GetEuler(2);
double psi = fdm.GetPropagate()->GetEuler(3);
// TODO check that this works properly
constraints.gamma = theta;
//if (thruster0->GetType()==FGThruster::ttPropeller)
//prompt("\tvariable prop pitch?\t\t",variablePropPitch);
// FIXME, enable
// mode menu
while (1)
{
//prompt("\tmode",mode);
constraints.rollRate = 0;
constraints.pitchRate = 0;
constraints.yawRate = 0;
if (mode == tLongitudinal) break;
else if (mode == tRoll)
{
prompt("\troll rate, rad/s",constraints.rollRate);
prompt("\tstability axis roll",constraints.stabAxisRoll);
// TODO check that this works properly
constraints.rollRate = fdm.GetAuxiliary()->GetEulerRates(1);
constraints.stabAxisRoll = true; // FIXME, make this an option
break;
}
else if (mode == tPullup)
{
prompt("\tpitch rate, rad/s",constraints.pitchRate);
// TODO check that this works properly
constraints.pitchRate = fdm.GetAuxiliary()->GetEulerRates(2);
break;
}
else if (mode == tTurn)
{
//prompt("\tyaw rate, rad/s",constraints.yawRate);
// TODO check that this works properly
double gd=fdm.GetInertial()->gravity();
constraints.yawRate = tan(phi)*gd*cos(theta)/constraints.velocity;
break;
}
else {
std::cerr << "\tunknown mode: " << mode << std::endl;
exit(1);
}
}
// solver properties
// TODO make these options
//std::cout << "\nsolver properties: " << std::endl;
//std::cout << std::scientific;
//prompt("\tshow converge status?\t",showConvergeStatus);
//prompt("\tshow simplex?\t\t",showSimplex);
//prompt("\tpause?\t\t\t",pause);
//prompt("\trelative tolerance\t",rtol);
//prompt("\tabsolute tolerance\t",abstol);
//prompt("\tmax iterations\t\t",iterMax);
//prompt("\tconvergence speed\t",speed);
//prompt("\trandomization ratio\t",random);
//std::cout << std::fixed;
// initial solver state
int n = 6;
std::vector<double> initialGuess(n), lowerBound(n), upperBound(n), initialStepSize(n);
lowerBound[0] = 0; //throttle
lowerBound[1] = -1; // elevator
lowerBound[2] = -20*M_PI/180; // alpha
lowerBound[3] = -1; // aileron
lowerBound[4] = -1; // rudder
lowerBound[5] = -20*M_PI/180; // beta
upperBound[0] = 1; //throttle
upperBound[1] = 1; // elevator
upperBound[2] = 20*M_PI/180; // alpha
upperBound[3] = 1; // aileron
upperBound[4] = 1; // rudder
upperBound[5] = 20*M_PI/180; // beta
initialStepSize[0] = 0.2; //throttle
initialStepSize[1] = 0.1; // elevator
initialStepSize[2] = 0.1; // alpha
initialStepSize[3] = 0.1; // aileron
initialStepSize[4] = 0.1; // rudder
initialStepSize[5] = 0.1; // beta
initialGuess[0] = 0.5; // throttle
initialGuess[1] = 0; // elevator
initialGuess[2] = 0; // alpha
initialGuess[3] = 0; // aileron
initialGuess[4] = 0; // rudder
initialGuess[5] = 0; // beta
// solve
FGTrimmer trimmer(fdm, constraints);
Callback callback(fileName,&trimmer);
FGNelderMead * solver;
try
{
solver = new FGNelderMead(trimmer,initialGuess,
lowerBound, upperBound, initialStepSize,iterMax,rtol,
abstol,speed,random,showConvergeStatus,showSimplex,pause,&callback);
while(solver->status()==1) solver->update();
}
catch (const std::runtime_error & e)
{
std::cout << e.what() << std::endl;
//exit(1);
}
// output
try
{
trimmer.printSolution(solver->getSolution()); // this also loads the solution into the fdm
std::cout << "\nfinal cost: " << std::scientific << std::setw(10) << trimmer.eval(solver->getSolution()) << std::endl;
}
catch(std::runtime_error & e)
{
std::cout << "caught std::runtime error" << std::endl;
std::cout << "exception: " << e.what() << std::endl;
exit(1);
}
time_trimDone = std::clock();
std::cout << "\ntrim computation time: " << (time_trimDone - time_start)/double(CLOCKS_PER_SEC) << "s \n" << std::endl;
//std::cout << "\nsimulating flight to determine trim stability" << std::endl;
//std::cout << "\nt = 5 seconds" << std::endl;
//for (int i=0;i<5*120;i++) fdm.Run();
//trimmer.printState();
//std::cout << "\nt = 10 seconds" << std::endl;
//for (int i=0;i<5*120;i++) fdm.Run();
//trimmer.printState();
std::cout << "\nlinearization: " << std::endl;
FGStateSpace ss(fdm);
ss.x.add(new FGStateSpace::Vt);
ss.x.add(new FGStateSpace::Alpha);
ss.x.add(new FGStateSpace::Theta);
ss.x.add(new FGStateSpace::Q);
if (thruster0->GetType()==FGThruster::ttPropeller)
{
ss.x.add(new FGStateSpace::Rpm0);
if (variablePropPitch) ss.x.add(new FGStateSpace::PropPitch);
int numEngines = fdm.GetPropulsion()->GetNumEngines();
if (numEngines>1) ss.x.add(new FGStateSpace::Rpm1);
if (numEngines>2) ss.x.add(new FGStateSpace::Rpm2);
if (numEngines>3) ss.x.add(new FGStateSpace::Rpm3);
}
ss.x.add(new FGStateSpace::Beta);
ss.x.add(new FGStateSpace::Phi);
ss.x.add(new FGStateSpace::P);
ss.x.add(new FGStateSpace::Psi);
ss.x.add(new FGStateSpace::R);
ss.x.add(new FGStateSpace::Latitude);
ss.x.add(new FGStateSpace::Longitude);
ss.x.add(new FGStateSpace::Alt);
ss.u.add(new FGStateSpace::ThrottleCmd);
ss.u.add(new FGStateSpace::DaCmd);
ss.u.add(new FGStateSpace::DeCmd);
ss.u.add(new FGStateSpace::DrCmd);
// state feedback
ss.y = ss.x;
std::vector< std::vector<double> > A,B,C,D;
std::vector<double> x0 = ss.x.get(), u0 = ss.u.get();
std::vector<double> y0 = x0; // state feedback
std::cout << ss << std::endl;
ss.linearize(x0,u0,y0,A,B,C,D);
int width=10;
std::cout.precision(3);
std::cout
<< std::fixed
<< std::right
<< "\nA=\n" << std::setw(width) << A
<< "\nB=\n" << std::setw(width) << B
<< "\nC=\n" << std::setw(width) << C
<< "\nD=\n" << std::setw(width) << D
<< std::endl;
// write scicoslab file
std::ofstream scicos(std::string(aircraft+"_lin.sce").c_str());
scicos.precision(10);
width=20;
scicos
<< std::scientific
<< aircraft << ".x0=..\n" << std::setw(width) << x0 << ";\n"
<< aircraft << ".u0=..\n" << std::setw(width) << u0 << ";\n"
<< aircraft << ".sys = syslin('c',..\n"
<< std::setw(width) << A << ",..\n"
<< std::setw(width) << B << ",..\n"
<< std::setw(width) << C << ",..\n"
<< std::setw(width) << D << ");\n"
<< aircraft << ".tfm = ss2tf(" << aircraft << ".sys);\n"
<< std::endl;
time_linDone = std::clock();
std::cout << "\nlinearization computation time: " << (time_linDone - time_trimDone)/double(CLOCKS_PER_SEC) << " s\n" << std::endl;
}