private static void Print(AscentSimulationGoal goal, LookAheadState lookAheadState, State state, AscentSimulationControl guess, double bestScore)
{
Console.WriteLine("{0,1:F} {1,1:F}, {2,2:F}/{3,2:F}: g {4,1:F}, d {5,1:F}, mass left {6,2:F} kg - score {7,3:F}/{8,3:F}",
(lookAheadState.Periapsis) / 1000.0,
(lookAheadState.Apoapsis) / 1000.0,
(state.Position.Length - Constants.EarthRadius) / 1000.0,
state.ReachedAltitude / 1000.0,
state.LossesToGravity,
state.LossesToDrag,
guess.Rocket.TotalFuelMass() - state.ExpendedMass,
ComputeScore(goal, lookAheadState, state, guess),
bestScore
);
}
public AscentOptimization(double surfaceVelocity, AscentSimulationGoal goal, AscentSimulationControl initialGuess, State initialState, double timeStep, int microSteps, double bestScore = -1)
{
Goal = goal;
InitialGuess = initialGuess;
BestGuess = initialGuess;
InitialState = initialState;
TimeStep = timeStep;
MicroSteps = microSteps;
SurfaceVelocity = surfaceVelocity;
LookAheadState lookAheadState;
State state;
BestScore = bestScore >= 0 ? bestScore : Tasklet(BestGuess, 0, 0.0, false, out BestGuess, out lookAheadState, out state);
}
public static double ComputeScore(AscentSimulationGoal goal, LookAheadState lookAheadState, State state, AscentSimulationControl guess)
{
const double distanceScaling = 10.0;
double distanceFromApoapsisGoal = lookAheadState.Apoapsis / 1000.0 - goal.Apoapsis;
double distanceFromPeriapsisGoal = lookAheadState.Periapsis / 1000.0 - goal.Periapsis;
double distanceFromReachedAltitudeGoal = state.ReachedAltitude / 1000.0 - goal.Periapsis;
double distanceFromAltitudeGoal = (state.Position.Length - Constants.EarthRadius) / 1000.0 - goal.Periapsis;
double totalFuel = guess.Rocket.TotalFuelMass();
double fuelLeft = (totalFuel - state.ExpendedMass) / totalFuel;
return(ErrorFunction(distanceFromApoapsisGoal, distanceScaling)
+ ErrorFunction(distanceFromPeriapsisGoal, distanceScaling)
+ ErrorFunction(distanceFromReachedAltitudeGoal, distanceScaling)
+ ErrorFunction(distanceFromAltitudeGoal, distanceScaling)
+ fuelLeft * 5);
//+ ErrorFunction((guess.ThrustDuration1 + guess.ThrustDuration2) - guess.ThrustCutoff, 1);
}
public AscentSimulation(
AscentSimulationGoal goal,
AscentSimulationControl controlInput,
State initialState,
double windSurfaceVelocity = 0.0
)
{
Goal = goal;
ControlInput = controlInput;
States.Add(initialState);
_lastState = initialState;
_currentState = initialState;
InitialState = initialState;
WindSurfaceVelocity = windSurfaceVelocity;
for (var i = 0; i < LookAheadState.FuturePositions.Length; i++)
{
LookAheadState.FuturePositions[i] = initialState.Position;
}
}
public void TerminalGuidance(AscentSimulationGoal goal, double timestep, double altitude, double verticalSpeed, double periapsis, double apoapsis, out double pitch, out double thrust, out bool isDone)
{
double maxAngle = 15;
double minAngle = -5;
double offset = 5;
isDone = false;
if (periapsis > 0)
{
maxAngle = 30;
minAngle = -30;
}
if (periapsis > -500)
{
offset = 0;
}
const double kP = 5;
const double kI = 5;
double error = -verticalSpeed;
double P = error * kP;
aggregatedError += error * timestep;
double I = aggregatedError * kI;
double degrees = Clamp(P + I + offset, minAngle, maxAngle);
var periapsisIsGood = periapsis > goal.Periapsis - 2;
var apoapsisIsGood = apoapsis > goal.Apoapsis - 2 && apoapsis < goal.Apoapsis + 3;
var verticalVelocityIsGood = Math.Abs(verticalSpeed) < 15;
//Console.WriteLine("{0,2:F} km x {1,2:F} km, v: {2,2:F} m/s", periapsis, apoapsis, verticalSpeed);
if (periapsisIsGood && apoapsisIsGood)
{
//Console.WriteLine("CANCELLING THRUST: periapsisIsGood && apoapsisIsGood");
thrust = 0;
isDone = true;
}
else if (periapsisIsGood && verticalVelocityIsGood)
{
//Console.WriteLine("CANCELLING THRUST: periapsisIsGood && verticalVelocityIsGood");
thrust = 0;
isDone = true;
}
else
if (periapsis > goal.Periapsis - 10)
{
thrust = 0.05;
}
else
if (periapsis > 0)
{
thrust = 0.25;
}
else
if (periapsis > -100)
{
thrust = 0.5;
}
else
{
thrust = 1;
}
//Console.WriteLine("{0,2:F}/{1,2:F} {2,2:F}/{3,2:F} = {4,2:F}, {5,2:F}", periapsis, goal.Periapsis, apoapsis, goal.Apoapsis, thrust, degrees);
//double degrees = -verticalSpeed;
//Console.WriteLine("{0,2:F}/{1,2:F} @ {2,2:F}, {3,2:F} deg, P{4,2:F} I{5,2:F}", altitude, verticalSpeed, P + I + 5, degrees, P, I);
pitch = Radian(degrees);
}
