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 StepOutput <GPSFilter2DSample> Filter(GPSObservation obs, GPSFilter2DInput input)
{
var inputFromPrev = new StepInput <Matrix>(_prevEstimate);
StepOutput <Matrix> result = CalculateNext(inputFromPrev, obs, input);
_prevEstimate = result;
return(ToFilterResult(result));
}
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,
});
}
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 IList <StepOutput <float> > Filter(IList <float> samples)
{
var result = new List <StepOutput <float> >();
// Initial state condition
const float x0 = 1.0f;
const float postX0 = 1.5f;
const float postP0 = 1.0f;
StepOutput <float> first = InitializeFirstStep(x0, postX0, postP0);
result.Add(first);
for (int i = 1; i < samples.Count; i++)
{
StepOutput <float> prev = result.Last();
result.Add(CalculateNext(new StepInput <float>(prev)));
}
return(result);
}
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);
}
