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