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,
});
}