public ScenarioTestResult(ScenarioIntermediaryTestResult i, Scenario scenario, HelicopterBase helicopter,
GameTime gameTime, TestScenarioEndTrigger endTrigger)
{
Autopilot = new AutopilotConfiguration
{
MaxHVelocity = helicopter.Autopilot.MaxHVelocity,
PIDSetup = helicopter.Autopilot.PIDSetup.Clone()
};
AvgEstimatedPositionError = i.AccEstimatedPositionError / i.UpdateCount;
AvgHeightAboveGround = i.AccHeightAboveGround / i.UpdateCount;
AvgVelocity = i.AccVelocity / i.UpdateCount;
Duration = gameTime.TotalGameTime - i.StartTime;
EndTrigger = endTrigger;
FlightLog = new List <HelicopterLogSnapshot>(helicopter.Log);
MaxEstimatedPositionError = i.MaxEstimatedPositionError;
MaxHeightAboveGround = i.MaxHeightAboveGround;
MaxVelocity = i.MaxVelocity;
MinEstimatedPositionError = i.MinEstimatedPositionError;
MinHeightAboveGround = i.MinHeightAboveGround;
Scenario = scenario;
Sensors = new SensorSpecifications
{
AccelerometerStdDev = DefaultAutopilotConfiguration.AccelerometerStdDev,
GPSVelocityStdDev = DefaultAutopilotConfiguration.GPSVelocityStdDev,
GPSPositionStdDev = DefaultAutopilotConfiguration.GPSPositionStdDev
};
}
public ScenarioTestResult(ScenarioIntermediaryTestResult i, Scenario scenario, HelicopterBase helicopter,
GameTime gameTime, TestScenarioEndTrigger endTrigger)
{
Autopilot = new AutopilotConfiguration
{
MaxHVelocity = helicopter.Autopilot.MaxHVelocity,
PIDSetup = helicopter.Autopilot.PIDSetup.Clone()
};
AvgEstimatedPositionError = i.AccEstimatedPositionError/i.UpdateCount;
AvgHeightAboveGround = i.AccHeightAboveGround/i.UpdateCount;
AvgVelocity = i.AccVelocity/i.UpdateCount;
Duration = gameTime.TotalGameTime - i.StartTime;
EndTrigger = endTrigger;
FlightLog = new List<HelicopterLogSnapshot>(helicopter.Log);
MaxEstimatedPositionError = i.MaxEstimatedPositionError;
MaxHeightAboveGround = i.MaxHeightAboveGround;
MaxVelocity = i.MaxVelocity;
MinEstimatedPositionError = i.MinEstimatedPositionError;
MinHeightAboveGround = i.MinHeightAboveGround;
Scenario = scenario;
Sensors = new SensorSpecifications
{
AccelerometerStdDev = DefaultAutopilotConfiguration.AccelerometerStdDev,
GPSVelocityStdDev = DefaultAutopilotConfiguration.GPSVelocityStdDev,
GPSPositionStdDev = DefaultAutopilotConfiguration.GPSPositionStdDev
};
}
private void Reset()
{
// Note: We got an odd bug that increased the initial estimate of the kalman filter the longer the simulation had run
// so that after running several scenarios in sequence, the initial estimate for each reset got worse.
// Not sure why, but resetting the simulation time prevents this behavior. Must be some resetting that I have missed in
// my sensor estimated state or sensors, but can't find where.
_accumulatedSimulatorTime = TimeSpan.Zero;
if (IsTestMode)
{
_intermediaryTestResult = new ScenarioIntermediaryTestResult
{
StartTime = _accumulatedSimulatorTime,
};
}
// Synchronize list of game components with currently selected scenario
UpdateGameComponents();
NavigationMap heightmap = UpdateTerrain(_scenario);
HelicopterScenario heliScenario = _scenario.HelicopterScenarios[0]; // TODO Multiple helicopters
// Setting new height values implicitly resets the physics engine
_terrainCollision.SetHeightValues(heightmap.HeightValues);
// _terrainCollision.Reset();
_helicopter.Autopilot.MaxHVelocity = _testMaxHVelocityIter.Current;
_helicopter.Reset(_accumulatedSimulatorTime, heliScenario, heightmap);
}
private void UpdateIntermediaryTestResult()
{
var currentLogEntry = _helicopter.Log.Last();
PhysicalHeliState trueState = currentLogEntry.True;
PhysicalHeliState estimatedState = currentLogEntry.Estimated;
ScenarioIntermediaryTestResult i = _intermediaryTestResult;
float estimationError = Vector3.Distance(estimatedState.Position, trueState.Position);
float heightAboveGround = trueState.Position.Y - currentLogEntry.GroundAltitude;
i.AccEstimatedPositionError += estimationError;
i.AccHeightAboveGround += heightAboveGround;
i.AccVelocity += trueState.Velocity.Length();
i.MaxEstimatedPositionError = Math.Max(estimationError, i.MaxEstimatedPositionError);
i.MinEstimatedPositionError = Math.Min(estimationError, i.MinEstimatedPositionError);
i.MaxHeightAboveGround = Math.Max(heightAboveGround, i.MaxHeightAboveGround);
i.MinHeightAboveGround = Math.Min(heightAboveGround, i.MinHeightAboveGround);
i.MaxVelocity = Math.Max(trueState.Velocity.Length(), i.MaxVelocity);
i.UpdateCount++;
_intermediaryTestResult = i;
}
