Example #1
0
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;
}
Example #2
0
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;
}