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