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