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 }