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