public GPSINSFilter(TimeSpan startTime, Vector3 startPos, Vector3 startVelocity, Quaternion orientation,
SensorSpecifications sensorSpecifications)
{
_startTime = startTime;
_orientation = orientation;
_sensorSpecifications = sensorSpecifications;
X0 = Matrix.Create(new double[n, 1]
{
{ startPos.X }, { startPos.Y }, { startPos.Z }, // Position
{ startVelocity.X }, { startVelocity.Y }, { startVelocity.Z }, // Velocity
{ _orientation.X }, { _orientation.Y }, { _orientation.Z }, { _orientation.W }, // Quaternion
});
// Make sure we don't reference the same object, but rather copy its values.
// It is possible to set PostX0 to a different state than X0, so that the initial guess
// of state is wrong.
PostX0 = X0.Clone();
// We use a very low initial estimate for error covariance, meaning the filter will
// initially trust the model more and the sensors/observations less and gradually adjust on the way.
// Note setting this to zero matrix will cause the filter to infinitely distrust all observations,
// so always use a close-to-zero value instead.
// Setting it to a diagnoal matrix of high values will cause the filter to trust the observations more in the beginning,
// since we say that we think the current PostX0 estimate is unreliable.
PostP0 = 0.001 * Matrix.Identity(n, n);
// Determine standard deviation of estimated process and observation noise variance
// Process noise (acceleromters, gyros, etc..)
_stdDevW = new Vector(new double[n]
{
_sensorSpecifications.AccelerometerStdDev.Forward,
_sensorSpecifications.AccelerometerStdDev.Right,
_sensorSpecifications.AccelerometerStdDev.Up,
0, 0, 0, 0, 0, 0, 0
});
// Observation noise (GPS inaccuarcy etc..)
_stdDevV = new Vector(new double[m]
{
_sensorSpecifications.GPSPositionStdDev.X,
_sensorSpecifications.GPSPositionStdDev.Y,
_sensorSpecifications.GPSPositionStdDev.Z,
// 0.001000, 0.001000, 0.001000,
// 1000, 1000, 1000,
_sensorSpecifications.GPSVelocityStdDev.X,
_sensorSpecifications.GPSVelocityStdDev.Y,
_sensorSpecifications.GPSVelocityStdDev.Z,
});
I = Matrix.Identity(n, n);
_zeroMM = Matrix.Zeros(m);
_rand = new GaussianRandom();
_prevEstimate = GetInitialEstimate(X0, PostX0, PostP0);
}
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 async Task <IReadOnlyList <Sensor> > GetSensors()
{
var sensorSpecification = new SensorSpecifications();
var sensorList = await _uow.Repository <Sensor>().ListAsync(sensorSpecification);
return(sensorList);
}
public async Task <Sensor> GetSensor(int id)
{
var sensorSpecification = new SensorSpecifications(a => a.SensorId == id);
var sensor = await _uow.Repository <Sensor>().GetEntityWithSpec(sensorSpecification);
return(sensor);
}
public IMU(SensorSpecifications sensorSpecifications, Quaternion startOrientation, bool isPerfect)
: base(isPerfect)
{
_accelerometer = new Accelerometer3Axis(sensorSpecifications, isPerfect);
_gyroscope = new Gyroscope3Axis(sensorSpecifications, isPerfect);
AccumulatedOrientation = startOrientation;
}
public GPS(SensorSpecifications sensorSpecifications, bool isPerfect, bool useUpdateFrequency)
{
_sensorSpecifications = sensorSpecifications;
_isPerfect = isPerfect;
_useUpdateFrequency = useUpdateFrequency;
_prevUpdate = TimeSpan.MinValue;
_initStartTime = TimeSpan.Zero;
// Position = Vector3.Zero;
// Velocity = Vector3.Zero;
Ready = false;
}
public Accelerometer3Axis(SensorSpecifications sensorSpecifications, bool isPerfect) : base(isPerfect)
{
_gaussRand = new GaussianRandom();
_frequency = sensorSpecifications.AccelerometerFrequency;
_rmsNoiseXY = sensorSpecifications.AccelerometerStdDev.Forward;
_rmsNoiseZ = sensorSpecifications.AccelerometerStdDev.Up;
_timeBetweenUpdates = (_frequency > 0)
? TimeSpan.FromSeconds(1.0 / _frequency)
: TimeSpan.Zero;
_isInitialized = false;
}
public SensorModel(SensorSpecifications sensorSpecifications, NavigationMap map, Vector3 startPosition, Quaternion startOrientation)
{
_sensorSpecifications = sensorSpecifications;
Console.WriteLine("Using sensors:\n" + sensorSpecifications);
// Instantiate sensors and populate properties and sensor list
const bool isPerfect = false;
_sensors = new List <ISensor>
{
(GPS = new GPS(sensorSpecifications, isPerfect, true)),
(IMU = new IMU(sensorSpecifications, startOrientation, isPerfect)),
(Magnetometer = new Magnetometer3Axis(sensorSpecifications, isPerfect)),
(GroundRangeFinder = new SonicRangeFinder(sensorSpecifications, isPerfect, map)),
(PressureGauge = new StaticPressureGauge(sensorSpecifications, isPerfect)),
};
}
public HelicopterBase(Game game, TestConfiguration testConfiguration, TerrainCollision collision,
ICameraProvider cameraProvider, BasicEffect effect, SunlightParameters skyParams, HelicopterScenario scenario,
bool playEngineSound, bool isPlayerControlled, bool drawText)
: base(game)
{
if (game == null || cameraProvider == null || effect == null || skyParams == null)
{
throw new ArgumentNullException("", @"One or several of the necessary arguments were null!");
}
_game = game;
_testConfiguration = testConfiguration;
_sensorSpecifications = testConfiguration.Sensors;
_collision = collision;
_flyBySensors = testConfiguration.FlyBySensors;
if (_collision != null)
{
_collision.CollidedWithTerrain += gameTime => Crashed(gameTime);
}
_basicEffect = effect;
_skyParams = skyParams;
_scenario = scenario;
_drawText = drawText;
_cameraProvider = cameraProvider;
IsPlayerControlled = isPlayerControlled;
_playEngineSound = playEngineSound;
_estimatedState = new HeliState();
PIDSetup pidSetup = SimulatorResources.GetPIDSetups()[0];
Autopilot = new Autopilot(_scenario.Task, pidSetup);
Log = new List <HelicopterLogSnapshot>();
}
public TestConfiguration()
{
FlyBySensors = false;
UsePerfectSensors = false;
UseGPS = true;
UseINS = true;
UseRangeFinder = true;
TestScenarios = new List <Scenario>();
MaxHVelocities = new List <float> {
DefaultAutopilotConfiguration.DefaultMaxHVelocity
};
Sensors = new SensorSpecifications
{
AccelerometerStdDev = DefaultAutopilotConfiguration.AccelerometerStdDev,
AccelerometerFrequency = 60,
GPSPositionStdDev = DefaultAutopilotConfiguration.GPSPositionStdDev,
GPSVelocityStdDev = DefaultAutopilotConfiguration.GPSVelocityStdDev,
OrientationAngleNoiseStdDev = 0,
};
}
private static SensorSpecifications ParseSensorSpecifications(XmlNode sensorsNode)
{
XmlNode accelerometerNode = sensorsNode.SelectSingleNode("Accelerometer");
XmlNode accelStdDevNode = accelerometerNode.SelectSingleNode("NoiseStdDev");
XmlNode gpsPosStdDevNode = sensorsNode.SelectSingleNode("GPSPositionAxisStdDev");
XmlNode gpsVelStdDevNode = sensorsNode.SelectSingleNode("GPSVelocityAxisStdDev");
var r = new SensorSpecifications
{
AccelerometerFrequency = float.Parse(accelerometerNode.SelectSingleNode("Frequency").InnerText),
AccelerometerStdDev = new ForwardRightUp(
float.Parse(accelStdDevNode.SelectSingleNode("Forward").InnerText),
float.Parse(accelStdDevNode.SelectSingleNode("Right").InnerText),
float.Parse(accelStdDevNode.SelectSingleNode("Up").InnerText)),
GPSPositionStdDev = new Vector3(float.Parse(gpsPosStdDevNode.InnerText)),
GPSVelocityStdDev = new Vector3(float.Parse(gpsVelStdDevNode.InnerText)),
OrientationAngleNoiseStdDev = float.Parse(sensorsNode.SelectSingleNode("OrientationAngleNoiseStdDev").InnerText),
};
return(r);
}
public SonicRangeFinder(SensorSpecifications sensorSpecifications, bool isPerfect, NavigationMap map)
: base(isPerfect)
{
_map = map;
_relativeRangeDirection = Vector3.Down;
}
/// <summary></summary>
public StaticPressureGauge(SensorSpecifications sensorSpecifications, bool isPerfect)
: base(isPerfect)
{
_pressureProvider = new StaticPressureProvider();
}
public Magnetometer3Axis(SensorSpecifications sensorSpecifications, bool isPerfect)
: base(isPerfect)
{
// TODO History length?
_sampleHistory = new RingBuffer <Axes>(100);
}
public Gyroscope3Axis(SensorSpecifications sensorSpecifications, bool isPerfect) : base(isPerfect)
{
_sensorSpecifications = sensorSpecifications;
}
/// <summary></summary>
public void Filter2DTest(SensorSpecifications sensorSpecifications)
{
try
{
if (Chart == null)
{
return;
}
// Add test Lines to demonstrate the control
Chart.Primitives.Clear();
throw new NotImplementedException(
"input.Orientation is not working any more as we no longer uses deltas but rather explicit orientation as input. We need to update orientation for each iteration for this to work again.");
// var startPos = new Vector3(0, 0, 0);
// var startVelocity = new Vector3(1, 0, 0);
// var input = new GPSINSInput
// {
// AccelerationWorld = new Vector3(0, +0.02f, 0),
// Orientation = Quaternion.Identity,
// PitchDelta = 0,
// RollDelta = +MathHelper.ToRadians(10),
// YawDelta = 0,
// PitchRate = 0,
// RollRate = +MathHelper.ToRadians(10),
// YawRate = 0,
// };
// var filter = new GPS_INSFilter(TimeSpan.Zero, startPos, startVelocity, 0, 0, 0);
// var filter = new GPSINSFilter(TimeSpan.Zero, startPos, startVelocity,
// Quaternion.CreateFromYawPitchRoll(0, 0, 0), sensorSpecifications);
// TODO GPS noise?
//
// var inValues = new List<GPSINSObservation>();
// for (int t = 0; t < 100; t++)
// {
// var obs = new GPSINSObservation
// {
// GPSPosition = startVelocity*t + 0.5f*input.AccelerationWorld*t*t,
// Time = TimeSpan.FromSeconds(t)
// };
//
//
// inValues.Add(obs);
// }
//
//
// var outValues = new List<StepOutput<GPSINSOutput>>();
// foreach (GPSINSObservation observation in inValues)
// outValues.Add(filter.Filter(observation, input));
//
// IEnumerable<StepOutput<GPS_INSFilter2DInput>> outValues = filter.Filter(inValues);
//
// AddLineXY("N-Window Smoothing", Colors.DarkGoldenrod, originalZValues);//smoothedZValues);
// AddLineXY("Noise W", Colors.White, outValues.Select(element => element.W));
// AddLineXY("Noise V", Colors.LightGray, outValues.Select(e => e.V.Position));
// AddLineXY("k", Colors.Cyan, outValues.Select(element => element.K));
// AddLineXY("A Priori X", Colors.LightBlue, outValues.Select(e => e.PriX.Position));
//
// TODO Commented out after converting to quaternions. Need to compute Roll somehow.
// AddLineXY("Roll", Colors.DeepPink, outValues.Select(e => e.PostX.Orientation.Roll));
//
// SwordfishGraphHelper.AddLineXY(Chart, "Estimate", Colors.Navy, outValues.Select(e => e.PostX.Position));
// SwordfishGraphHelper.AddLineXY(Chart, "True", Colors.DarkRed, outValues.Select(e => e.X.Position));
// SwordfishGraphHelper.AddLineXY(Chart, "GPS", Colors.Yellow, outValues.Select(e => e.Z.Position));
// AddLineXY(Chart, "True", Color.FromArgb(50, 150, 0, 0), inValues.Select(e => e.Position));
//
// Chart.Title = "Filter2DTest()";
// Chart.XAxisLabel = "X [m]";
// Chart.YAxisLabel = "Y [m]";
//
// Chart.RedrawPlotLines();
}
catch (Exception e)
{
MessageBox.Show(e.ToString());
}
}
