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);
}
private Matrix GetNoiseMatrix(Vector stdDev, int rows)
{
var matrixData = new double[rows, 1];
for (int r = 0; r < rows; r++)
{
matrixData[r, 0] = _rand.NextGaussian(0, stdDev[r]);
}
return(Matrix.Create(matrixData));
}
/// <summary>
/// Transform sensor measurements to output matrix
/// </summary>
/// <param name="s"></param>
/// <returns></returns>
private static Matrix ToOutputMatrix(GPSObservation s)
{
return(Matrix.Create(new double[m, 1]
{
{ s.Position.X },
{ s.Position.Y },
{ s.Position.Z },
// {0}, {0}, {0}, // Velocity
// {0}, {0}, {0}, // Acceleration
// {s.Time.TotalSeconds},
}));
}
/// <summary>
/// Transform sensor measurements to output matrix
/// </summary>
/// <param name="s"></param>
/// <returns></returns>
private static Matrix ToObservationMatrix(GPSINSObservation s)
{
return(Matrix.Create(new double[m, 1]
{
{ s.GPSPosition.X },
{ s.GPSPosition.Y },
{ s.GPSPosition.Z },
{ s.GPSHVelocity.X },
{ s.GPSHVelocity.Y },
{ s.GPSHVelocity.Z },
}));
}
private void UpdateMatrices(TimeSpan timeTotal, TimeSpan timeDelta)
{
double t = timeTotal.TotalSeconds;
double dt = timeDelta.TotalSeconds;
double posByV = dt; // p=vt
double posByA = 0.5 * dt * dt; // p=0.5at^2
double velByA = t; // v=at
// World state transition matrix
A = Matrix.Create(new double[n, n]
{
{ 1, 0, 0, posByV, 0, 0, posByA, 0, 0 }, // Px
{ 0, 1, 0, 0, posByV, 0, 0, posByA, 0 }, // Py
{ 0, 0, 1, 0, 0, posByV, 0, 0, posByA }, // Pz
{ 0, 0, 0, 1, 0, 0, velByA, 0, 0 }, // Vx
{ 0, 0, 0, 0, 1, 0, 0, velByA, 0 }, // Vy
{ 0, 0, 0, 0, 0, 1, 0, 0, velByA }, // Vz
{ 0, 0, 0, 0, 0, 0, 0, 0, 0 }, // Ax
{ 0, 0, 0, 0, 0, 0, 0, 0, 0 }, // Ay
{ 0, 0, 0, 0, 0, 0, 0, 0, 0 }, // Az
});
B = Matrix.Create(new double[n, p]
{
{ posByV, 0, 0, posByA, 0, 0 }, // Px
{ 0, posByV, 0, 0, posByA, 0 }, // Py
{ 0, 0, posByV, 0, 0, posByA }, // Pz
{ 1, 0, 0, velByA, 0, 0 }, // Vx
{ 0, 1, 0, 0, velByA, 0 }, // Vy
{ 0, 0, 1, 0, 0, velByA }, // Vz
{ 0, 0, 0, 1, 0, 0 }, // Ax
{ 0, 0, 0, 0, 1, 0 }, // Ay
{ 0, 0, 0, 0, 0, 1 }, // Az
});
// u = Matrix.Create(new double[p, 1]
// {
// {0}, // Px
// {0}, // Py
// {0}, // Pz
// {0}, // Vx
// {0}, // Vy
// {0}, // Vz
// {0}, // Ax
// {0}, // Ay
// {0}, // Az
// });
w = GetNoiseMatrix(_stdDevW, n);
v = GetNoiseMatrix(_stdDevV, m);
}
private static Matrix ToInputMatrix(GPSFilter2DInput input)
{
return(Matrix.Create(new double[p, 1]
{
// {0}, // Px
// {0}, // Py
// {0}, // Pz
{ input.Velocity.X }, // Vx
{ input.Velocity.Y }, // Vy
{ input.Velocity.Z }, // Vz
{ input.Acceleration.X }, // Ax
{ input.Acceleration.Y }, // Ay
{ input.Acceleration.Z }, // Az
}));
}
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 },
}));
}
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);
}
// End FindTraceandNorm(double[,][,] Matrix, double[][] GlobalxVector, ref double Trace, ref double Norm)
public static bool SolveMatrix(double[][] Answer, Desertwind Solution)
{
var ConventionalMatrix = new double[Hotsun.npar, Hotsun.npar];
var ConventionalFirst = new double[Hotsun.npar, 1];
var ConventionalAnswer = new double[Hotsun.npar][];
for (int GlobalIndex1 = 0; GlobalIndex1 < Hotsun.npar; GlobalIndex1++)
{
ConventionalAnswer[GlobalIndex1] = new double[1];
}
double[, ][,] Matrix;
if (Hotsun.FullSecondDerivative)
{
Matrix = Solution.ExactFullMatrix;
}
else
{
Matrix = Solution.FullMatrix;
}
// Set actual matrix
for (int GlobalIndex1 = 0; GlobalIndex1 < Hotsun.npar; GlobalIndex1++)
{
if (Hotsun.FixedParameter[GlobalIndex1][0])
{
ConventionalFirst[GlobalIndex1, 0] = 0.0;
}
else
{
ConventionalFirst[GlobalIndex1, 0] = Solution.first[GlobalIndex1][0] * Hotsun.sqdginv[GlobalIndex1][0];
}
for (int GlobalIndex2 = 0; GlobalIndex2 < Hotsun.npar; GlobalIndex2++)
{
double MatrixElement = Matrix[GlobalIndex1, GlobalIndex2][0, 0];
if (Hotsun.UseDiagonalScaling)
{
MatrixElement *= Hotsun.sqdginv[GlobalIndex1][0] * Hotsun.sqdginv[GlobalIndex2][0];
}
if (GlobalIndex1 == GlobalIndex2)
{
if (Hotsun.AddMarquardtQDynamically)
{
MatrixElement += Hotsun.Q;
}
}
ConventionalMatrix[GlobalIndex1, GlobalIndex2] = MatrixElement;
} // End GlobalIndex2
} // End GlobalIndex1
// Form ConventionalMatrix-1 * ConventionalFirst
LMatrix cMatrix = LMatrix.Create(ConventionalMatrix);
LMatrix RightHandSide = LMatrix.Create(ConventionalFirst);
var Fred = new LU(cMatrix);
LMatrix MatrixAnswer = Fred.Solve(RightHandSide);
ConventionalAnswer = MatrixAnswer;
for (int GlobalIndex1 = 0; GlobalIndex1 < Hotsun.npar; GlobalIndex1++)
{
Answer[GlobalIndex1][0] = ConventionalAnswer[GlobalIndex1][0] * Hotsun.sqdginv[GlobalIndex1][0];
}
bool success = true;
return(success);
}
// End AddupChisqContributions(ChisqFirstandSecond[] SubTotal, Desertwind TotalSolution)
public static void FindQlimits(Desertwind Solution, ref double Qhigh, ref double Qlow, ref int ReasontoStop1,
ref int ReasontoStop2)
{
if (Hotsun.FullSecondDerivative)
{
FindTraceandNorm(Solution.ExactFullMatrix, ref Hotsun.ChisqMatrixTrace, ref Hotsun.ChisqMatrixNorm);
}
else
{
FindTraceandNorm(Solution.FullMatrix, ref Hotsun.ChisqMatrixTrace, ref Hotsun.ChisqMatrixNorm);
}
var ConventionalMatrix = new double[Hotsun.npar, Hotsun.npar];
// Set Up Matrix to find eigenvalues
// Scale but do NOT add Q
double[, ][,] Matrix;
if (Hotsun.FullSecondDerivative)
{
Matrix = Solution.ExactFullMatrix;
}
else
{
Matrix = Solution.FullMatrix;
}
// Set actual matrix
for (int GlobalIndex1 = 0; GlobalIndex1 < Hotsun.npar; GlobalIndex1++)
{
for (int GlobalIndex2 = 0; GlobalIndex2 < Hotsun.npar; GlobalIndex2++)
{
double MatrixElement = Matrix[GlobalIndex1, GlobalIndex2][0, 0];
if (Hotsun.UseDiagonalScaling)
{
MatrixElement *= Hotsun.sqdginv[GlobalIndex1][0] * Hotsun.sqdginv[GlobalIndex2][0];
}
ConventionalMatrix[GlobalIndex1, GlobalIndex2] = MatrixElement;
} // End GlobalIndex2
} // End GlobalIndex1
// Find Minimum and Maximum eigenvalue of ConventionalMatrix
// Begin Added by smbeason 5/17/2009
LMatrix lmatrix = LMatrix.Create(ConventionalMatrix);
EigenvalueDecomposition eigenValueDecomp = lmatrix.EigenvalueDecomposition;
double minEigenValue = double.MaxValue;
double maxEigenValue = double.MinValue;
//Assuming you want on the real part...
for (int i = 0; i < eigenValueDecomp.RealEigenvalues.Length; i++)
{
minEigenValue = Math.Min(minEigenValue, eigenValueDecomp.RealEigenvalues[i]);
maxEigenValue = Math.Max(maxEigenValue, eigenValueDecomp.RealEigenvalues[i]);
}
// End Added by smbeason 5/17/2009
ReasontoStop1 = 1;
ReasontoStop2 = 1;
Qlow = minEigenValue;
Qhigh = maxEigenValue;
return;
}
private void UpdateTimeVaryingMatrices(TimeSpan timeDelta)
{
double dt = timeDelta.TotalSeconds;
double posByV = dt; // p=vt
double posByA = 0.5 * dt * dt; // p=0.5at^2
double velByA = dt; // v=at
// World state transition matrix.
// Update position and velocity from previous state.
// Previous state acceleration is neglected since current acceleration only depends on current input.
A = Matrix.Create(new double[n, n]
{
// Px Py Pz Vx Vy Vz Qx Qy Qz Qw
{ 1, 0, 0, posByV, 0, 0, 0, 0, 0, 0 }, // Px
{ 0, 1, 0, 0, posByV, 0, 0, 0, 0, 0 }, // Py
{ 0, 0, 1, 0, 0, posByV, 0, 0, 0, 0 }, // Pz
{ 0, 0, 0, 1, 0, 0, 0, 0, 0, 0 }, // Vx
{ 0, 0, 0, 0, 1, 0, 0, 0, 0, 0 }, // Vy
{ 0, 0, 0, 0, 0, 1, 0, 0, 0, 0 }, // Vz
// We don't handle transition of quaternions here due to difficulties. Using B instead.
{ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 }, // Quaternion X
{ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 }, // Quaternion Y
{ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 }, // Quaternion Z
{ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 }, // Quaternion W
});
AT = Matrix.Transpose(A);
// Input gain matrix.
// Acceleration forward/right/down
// Angular Rate Roll/Pitch/Heading
B = Matrix.Create(new double[n, p]
{
// Ax Ay Az Qx Qy Qz Qw
{ posByA, 0, 0, 0, 0, 0, 0 }, // Px
{ 0, posByA, 0, 0, 0, 0, 0 }, // Py
{ 0, 0, posByA, 0, 0, 0, 0 }, // Pz
{ velByA, 0, 0, 0, 0, 0, 0 }, // Vx
{ 0, velByA, 0, 0, 0, 0, 0 }, // Vy
{ 0, 0, velByA, 0, 0, 0, 0 }, // Vz
// Simply set new orientation directly by quaternion input
{ 0, 0, 0, 1, 0, 0, 0 }, // Quaternion X
{ 0, 0, 0, 0, 1, 0, 0 }, // Quaternion Y
{ 0, 0, 0, 0, 0, 1, 0 }, // Quaternion Z
{ 0, 0, 0, 0, 0, 0, 1 }, // Quaternion W
});
// TODO For simplicity we assume all acceleromter axes have identical standard deviations (although they don't)
float accelStdDev = _sensorSpecifications.AccelerometerStdDev.Forward;
float velocityStdDev = ((float)velByA) * accelStdDev;
float positionStdDev = ((float)posByA) * accelStdDev;
// Diagonal matrix with noise std dev
Q = Matrix.Diagonal(new Vector(new double[n]
{
positionStdDev, positionStdDev, positionStdDev,
velocityStdDev, velocityStdDev, velocityStdDev,
0, 0, 0, 0 // TODO Orientation has no noise, should be added later
}));
R = Matrix.Diagonal(_stdDevV); // Diagonal matrix with noise std dev
Q *= Matrix.Transpose(Q); // Convert standard deviations to variances
R *= Matrix.Transpose(R); // Convert standard deviations to variances
w = GetNoiseMatrix(_stdDevW, n);
v = GetNoiseMatrix(_stdDevV, m);
}
