public void ShouldExecuteThreeStepsAndReturnStepOutputs()
{
var stepDouble = A.Fake <IReservationStep>();
var stepDouble2 = A.Fake <IReservationStep>();
var stepDouble3 = A.Fake <IReservationStep>();
var stepOutputDouble = new StepOutput(null);
var stepOutputDouble2 = new StepOutput(new List <InputType> {
(InputType)(-1)
});
var stepOutputDouble3 = new StepOutput(new List <InputType> {
(InputType)(-1), (InputType)(-1)
});
var expectedStepOutputs = new List <StepOutput> {
stepOutputDouble, stepOutputDouble2, stepOutputDouble3
};
A.CallTo(() => stepDouble.Execute(_stepsInputs)).Returns(stepOutputDouble);
A.CallTo(() => stepDouble2.Execute(_stepsInputs)).Returns(stepOutputDouble2);
A.CallTo(() => stepDouble3.Execute(_stepsInputs)).Returns(stepOutputDouble3);
var stepOutputs = _subject.ExecuteSteps(
new List <IReservationStep> {
stepDouble, stepDouble2, stepDouble3
}, _stepsInputs);
stepOutputs.Should().BeEquivalentTo(expectedStepOutputs);
}
public void ShouldCreateNewStepOutputWithProvidedIncorrectInputTypes()
{
var incorrectInputTypes = new List <InputType> {
(InputType)(-1)
};
var stepOutput = new StepOutput(incorrectInputTypes);
stepOutput.IncorrectInputsTypes.Should().BeEquivalentTo(incorrectInputTypes);
}
public void ShouldReturnSuccessfulReservationResult()
{
const int dummyHotelId = 1;
var stepInputsDouble = new List <StepInput> {
A.Fake <StepInput>()
};
var stepOutputDouble = new StepOutput(new List <InputType>());
A.CallTo(() => _stepExecutorDouble.ExecuteSteps(A <List <IReservationStep> > ._, A <List <StepInput> > ._)).Returns(new List <StepOutput> {
stepOutputDouble
});
var reservationResult = _subject.ReserveHotel(dummyHotelId, stepInputsDouble);
reservationResult.IsSuccessful.Should().BeTrue();
}
public void ShouldReturnReservationResultWithIncorrectInputTypes()
{
const int dummyHotelId = 1;
var stepInputsDouble = new List <StepInput> {
A.Fake <StepInput>()
};
var incorrectInputTypesDouble = new List <InputType> {
(InputType)(-1)
};
var stepOutputDouble = new StepOutput(incorrectInputTypesDouble);
A.CallTo(() => _stepExecutorDouble.ExecuteSteps(A <List <IReservationStep> > ._, A <List <StepInput> > ._)).Returns(new List <StepOutput> {
stepOutputDouble
});
var reservationResult = _subject.ReserveHotel(dummyHotelId, stepInputsDouble);
reservationResult.IncorrectInputTypes.Should().BeEquivalentTo(new ReadOnlyCollection <InputType>(incorrectInputTypesDouble));
}
private static StepOutput<GPSINSOutput> ToFilterResult(StepOutput<Matrix> step)
{
return new StepOutput<GPSINSOutput>
{
X = EstimateMatrixToFilterResult(step.X),
PriX = EstimateMatrixToFilterResult(step.PriX),
PostX = EstimateMatrixToFilterResult(step.PostX),
Z = ObservationMatrixToFilterResult(step.Z),
Time = step.Time,
};
}
public StepOutput<GPSINSOutput> Filter(GPSINSObservation obs, GPSINSInput input)
{
var inputFromPrev = new StepInput<Matrix>(_prevEstimate);
StepOutput<Matrix> result = CalculateNext(inputFromPrev, obs, input);
_prevEstimate = result;
return ToFilterResult(result);
}
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 StepInput(StepOutput <T> prev)
{
PostP = prev.PostP;
PostX = prev.PostX;
X = prev.X;
}
private static StepOutput<GPSFilter2DSample> ToFilterResult(StepOutput<Matrix> step)
{
return new StepOutput<GPSFilter2DSample>
{
X = new GPSFilter2DSample {Position = EstimateMatrixToFilterResult(step.X).Position,},
PriX = new GPSFilter2DSample {Position = EstimateMatrixToFilterResult(step.PriX).Position,},
PostX = new GPSFilter2DSample {Position = EstimateMatrixToFilterResult(step.PostX).Position,},
Z = new GPSFilter2DSample {Position = EstimateMatrixToFilterResult(step.Z).Position,},
Time = step.Time,
};
}
public GPSFilter2D(GPSObservation startState)
{
X0 = Matrix.Create(new double[n,1]
{
{startState.Position.X}, {startState.Position.Y}, {startState.Position.Z},
// Position
{0}, {0}, {0}, // Velocity
{0}, {0}, {0}, // Acceleration
});
PostX0 = X0.Clone();
/* Matrix.Create(new double[n, 1]
{
{startState.Position.X}, {startState.Position.Y}, {startState.Position.Z}, // Position
{1}, {0}, {0}, // Velocity
{0}, {1}, {0}, // Acceleration
});*/
// Start by assuming no covariance between states, meaning position, velocity, acceleration and their three XYZ components
// have no correlation and behave independently. This not entirely true.
PostP0 = Matrix.Identity(n, n);
// Refs:
// http://www.romdas.com/technical/gps/gps-acc.htm
// http://www.sparkfun.com/datasheets/GPS/FV-M8_Spec.pdf
// http://onlinestatbook.com/java/normalshade.html
// Assuming GPS Sensor: FV-M8
// Cold start: 41s
// Hot start: 1s
// Position precision: 3.3m CEP (horizontal circle, half the points within this radius centred on truth)
// Position precision (DGPS): 2.6m CEP
// const float coVarQ = ;
// _r = covarianceR;
// Determine standard deviation of estimated process and observation noise variance
// Position process noise
_stdDevW = Vector.Zeros(n); //(float)Math.Sqrt(_q);
_rand = new GaussianRandom();
// Circle Error Probable (50% of the values are within this radius)
// const float cep = 3.3f;
// GPS position observation noise by standard deviation [meters]
// Assume gaussian distribution, 2.45 x CEP is approx. 2dRMS (95%)
// ref: http://www.gmat.unsw.edu.au/snap/gps/gps_survey/chap2/243.htm
// Found empirically by http://onlinestatbook.com/java/normalshade.html
// using area=0.5 and limits +- 3.3 meters
_stdDevV = new Vector(new double[m]
{
0,
ObservationNoiseStdDevY,
0,
// 0,
// 0,
// 0,
// 0,
// 0,
// 0,
// 0,
});
//Vector.Zeros(Observations);
//2, 2, 4.8926f);
H = Matrix.Identity(m, n);
I = Matrix.Identity(n, n);
Q = new Matrix(n, n);
R = Matrix.Identity(m, m);
_prevEstimate = GetInitialEstimate(X0, PostX0, PostP0);
}
public void Update(SimulationStepResults trueState, long elapsedMillis, long totalElapsedMillis,
JoystickOutput currentOutput)
{
#if XBOX
// TODO Xbox equivalent or fix the GPSINSFilter dependency on Iridium Math.Net
_isInitialized = true;
#else
// TODO If sensors are not ready we cannot produce an estimated state
// However, currently the helicopter starts mid-air so we have no time to wait for it to initialize.. so this is cheating.
// When the helicopter starts on the ground we can properly implement this process.
if (!_isInitialized)
{
if (!_sensors.Ready)
{
return;
}
_estimated.Position = _initialState.Position;
_estimated.Velocity = _initialState.Velocity;
_estimated.Acceleration = _initialState.Acceleration;
_estimated.Orientation = _initialState.Orientation;
_prevGPSPos = _sensors.GPS.Output.Position;
_gpsins = new GPSINSFilter(
TimeSpan.FromMilliseconds(totalElapsedMillis - elapsedMillis),
_initialState.Position,
_initialState.Velocity,
_initialState.Orientation,
_sensors.Specifications);
_isInitialized = true;
}
TimeSpan totalTime = TimeSpan.FromMilliseconds(totalElapsedMillis);
// Setup observations
var observations = new GPSINSObservation();
observations.Time = totalTime;
observations.RangeFinderHeightOverGround = _sensors.GroundRangeFinder.FlatGroundHeight; // TODO Update frequency? Noise?
if (_sensors.GPS.IsUpdated)
{
observations.GotGPSUpdate = true;
observations.GPSPosition = _sensors.GPS.Output.Position;
observations.GPSHVelocity = _sensors.GPS.Output.Velocity;
}
// Setup input to filter's internal model
var input = new GPSINSInput
{
AccelerationWorld = _sensors.IMU.AccelerationWorld,
Orientation = _sensors.IMU.AccumulatedOrientation,
// PitchDelta = _sensors.IMU.AngularDeltaBody.Radians.Pitch,
// RollDelta = _sensors.IMU.AngularDeltaBody.Radians.Roll,
// YawDelta = _sensors.IMU.AngularDeltaBody.Radians.Yaw,
// PitchRate = _sensors.IMU.Output.AngularRate.Radians.Pitch,
// RollRate = _sensors.IMU.Output.AngularRate.Radians.Roll,
// YawRate = _sensors.IMU.Output.AngularRate.Radians.Yaw,
};
// Estimate
StepOutput <GPSINSOutput> gpsinsEstimate = _gpsins.Filter(observations, input);
// Vector3 deltaPosition = gpsinsEstimate.PostX.Position - _currentEstimate.Position;
// if (deltaPosition.Length() > 40)
// deltaPosition = deltaPosition;
// var trueState = CheatingTrueState.Result;
GPSINSOutput observed = gpsinsEstimate.Z;
GPSINSOutput estimated = gpsinsEstimate.PostX;
GPSINSOutput blindEstimated = gpsinsEstimate.X;
_currentEstimate = new PhysicalHeliState(estimated.Orientation, estimated.Position, estimated.Velocity,
input.AccelerationWorld);
_currentBlindEstimate = new PhysicalHeliState(blindEstimated.Orientation, blindEstimated.Position, blindEstimated.Velocity,
input.AccelerationWorld);
_currentObservation = new PhysicalHeliState(observed.Orientation, observed.Position, observed.Velocity,
Vector3.Zero);
#endif
}
