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)); }
private Matrix ToInputMatrix(GPSINSInput input, TimeSpan time) { Vector3 worldAcceleration = input.AccelerationWorld; var q = input.Orientation; var inputOrientation = new Quaternion(q.X, q.Y, q.Z, q.W); // Periodically set an orientation error that will increase the estimation error to // make up for the missing orientation filtering. Perfect orientation hinders estimation error from accumulating significantly. // TODO Use simulation time instead if (time - _prevTimeOrientationNoiseGenerated > TimeSpan.FromSeconds(0.5f)) { _prevTimeOrientationNoiseGenerated = time; float angleStdDev = MathHelper.ToRadians(_sensorSpecifications.OrientationAngleNoiseStdDev); _noisyRotation = Quaternion.CreateFromYawPitchRoll(_gaussRand.NextGaussian(0, angleStdDev), _gaussRand.NextGaussian(0, angleStdDev), _gaussRand.NextGaussian(0, angleStdDev)); } var noisyOrientation = inputOrientation * _noisyRotation; return(Matrix.Create(new double[p, 1] { { worldAcceleration.X }, { worldAcceleration.Y }, { worldAcceleration.Z }, // {input.Orientation.X}, // {input.Orientation.Y}, // {input.Orientation.Z}, // {input.Orientation.W}, { noisyOrientation.X }, { noisyOrientation.Y }, { noisyOrientation.Z }, { noisyOrientation.W }, })); }
private Matrix ToInputMatrix(GPSINSInput input, TimeSpan time) { Vector3 worldAcceleration = input.AccelerationWorld; var q = input.Orientation; var inputOrientation = new Quaternion(q.X, q.Y, q.Z, q.W); // Periodically set an orientation error that will increase the estimation error to // make up for the missing orientation filtering. Perfect orientation hinders estimation error from accumulating significantly. // TODO Use simulation time instead if (time - _prevTimeOrientationNoiseGenerated > TimeSpan.FromSeconds(0.5f)) { _prevTimeOrientationNoiseGenerated = time; float angleStdDev = MathHelper.ToRadians(_sensorSpecifications.OrientationAngleNoiseStdDev); _noisyRotation = Quaternion.CreateFromYawPitchRoll(_gaussRand.NextGaussian(0, angleStdDev), _gaussRand.NextGaussian(0, angleStdDev), _gaussRand.NextGaussian(0, angleStdDev)); } var noisyOrientation = inputOrientation*_noisyRotation; return Matrix.Create(new double[p,1] { {worldAcceleration.X}, {worldAcceleration.Y}, {worldAcceleration.Z}, // {input.Orientation.X}, // {input.Orientation.Y}, // {input.Orientation.Z}, // {input.Orientation.W}, {noisyOrientation.X}, {noisyOrientation.Y}, {noisyOrientation.Z}, {noisyOrientation.W}, }); }
private StepOutput<Matrix> CalculateNext(StepInput<Matrix> prev, GPSINSObservation observation, GPSINSInput input) { TimeSpan time = observation.Time; TimeSpan timeDelta = time - _prevEstimate.Time; // Note: Use 0.0 and not 0 in order to use the correct constructor! // If no GPS update is available, then use a zero matrix to ignore the values in the observation. H = observation.GotGPSUpdate ? Matrix.Identity(m, n) : new Matrix(m, n, 0.0); HT = Matrix.Transpose(H); UpdateTimeVaryingMatrices(timeDelta); Matrix u = ToInputMatrix(input, time); // Ref equations: http://en.wikipedia.org/wiki/Kalman_filter#The_Kalman_filter // Calculate the state and the output Matrix x = A*prev.X + B*u; // +w; noise is already modelled in input data Matrix z = ToObservationMatrix(observation); //H * x + v;// m by 1 // Predictor equations Matrix PriX = A*prev.PostX + B*u; // n by 1 Matrix PriP = A*prev.PostP*AT + Q; // n by n Matrix residual = z - H*PriX; // m by 1 Matrix residualP = H*PriP*HT + R; // m by m // If residualP is zero matrix then set its inverse to zero as well. // This occurs if all observation standard deviations are zero // and this workaround will cause the Kalman gain to trust the process model entirely. Matrix residualPInv = Matrix.AlmostEqual(residualP, _zeroMM) // m by m ? _zeroMM : residualP.Inverse(); // Corrector equations Matrix K = PriP*Matrix.Transpose(H)*residualPInv; // n by m Matrix PostX = PriX + K*residual; // n by 1 Matrix PostP = (I - K*H)*PriP; // n by n var tmpPosition = new Vector3((float)PostX[0, 0], (float)PostX[1, 0], (float)PostX[2, 0]); // var tmpPrevPosition = new Vector3((float)prev.PostX[0, 0], (float)prev.PostX[1, 0], (float)prev.PostX[2, 0]); // Vector3 positionChange = tmpPosition - tmpPrevPosition; Vector3 gpsPositionTrust = new Vector3((float)K[0, 0], (float)K[1, 1], (float)K[2, 2]); Vector3 gpsVelocityTrust = new Vector3((float)K[3, 3], (float)K[4, 4], (float)K[5, 5]); // // Matrix tmpPriPGain = A*Matrix.Identity(10,10)*AT + Q; var tmpVelocity = new Vector3((float)x[3, 0], (float)x[4, 0], (float)x[5, 0]); var tmpAccel = new Vector3((float)x[6, 0], (float)x[7, 0], (float)x[8, 0]); return new StepOutput<Matrix> { K = K, PriX = PriX, PriP = PriP, PostX = PostX, PostP = PostP, PostXError = PostX - x, PriXError = PriX - x, X = x, Z = z, W = w, V = v, Time = 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 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 }
private StepOutput <Matrix> CalculateNext(StepInput <Matrix> prev, GPSINSObservation observation, GPSINSInput input) { TimeSpan time = observation.Time; TimeSpan timeDelta = time - _prevEstimate.Time; // Note: Use 0.0 and not 0 in order to use the correct constructor! // If no GPS update is available, then use a zero matrix to ignore the values in the observation. H = observation.GotGPSUpdate ? Matrix.Identity(m, n) : new Matrix(m, n, 0.0); HT = Matrix.Transpose(H); UpdateTimeVaryingMatrices(timeDelta); Matrix u = ToInputMatrix(input, time); // Ref equations: http://en.wikipedia.org/wiki/Kalman_filter#The_Kalman_filter // Calculate the state and the output Matrix x = A * prev.X + B * u; // +w; noise is already modelled in input data Matrix z = ToObservationMatrix(observation); //H * x + v;// m by 1 // Predictor equations Matrix PriX = A * prev.PostX + B * u; // n by 1 Matrix PriP = A * prev.PostP * AT + Q; // n by n Matrix residual = z - H * PriX; // m by 1 Matrix residualP = H * PriP * HT + R; // m by m // If residualP is zero matrix then set its inverse to zero as well. // This occurs if all observation standard deviations are zero // and this workaround will cause the Kalman gain to trust the process model entirely. Matrix residualPInv = Matrix.AlmostEqual(residualP, _zeroMM) // m by m ? _zeroMM : residualP.Inverse(); // Corrector equations Matrix K = PriP * Matrix.Transpose(H) * residualPInv; // n by m Matrix PostX = PriX + K * residual; // n by 1 Matrix PostP = (I - K * H) * PriP; // n by n var tmpPosition = new Vector3((float)PostX[0, 0], (float)PostX[1, 0], (float)PostX[2, 0]); // var tmpPrevPosition = new Vector3((float)prev.PostX[0, 0], (float)prev.PostX[1, 0], (float)prev.PostX[2, 0]); // Vector3 positionChange = tmpPosition - tmpPrevPosition; Vector3 gpsPositionTrust = new Vector3((float)K[0, 0], (float)K[1, 1], (float)K[2, 2]); Vector3 gpsVelocityTrust = new Vector3((float)K[3, 3], (float)K[4, 4], (float)K[5, 5]); // // Matrix tmpPriPGain = A*Matrix.Identity(10,10)*AT + Q; var tmpVelocity = new Vector3((float)x[3, 0], (float)x[4, 0], (float)x[5, 0]); var tmpAccel = new Vector3((float)x[6, 0], (float)x[7, 0], (float)x[8, 0]); return(new StepOutput <Matrix> { K = K, PriX = PriX, PriP = PriP, PostX = PostX, PostP = PostP, PostXError = PostX - x, PriXError = PriX - x, X = x, Z = z, W = w, V = v, Time = time, }); }