public StepOutput <GPSINSOutput> Filter(GPSINSObservation obs, GPSINSInput input)
        {
            var inputFromPrev          = new StepInput <Matrix>(_prevEstimate);
            StepOutput <Matrix> result = CalculateNext(inputFromPrev, obs, input);

            _prevEstimate = result;

            return(ToFilterResult(result));
        }
        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 },
            }));
        }
        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},
                                     });
        }
        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,
                       };
        }
        public StepOutput<GPSINSOutput> Filter(GPSINSObservation obs, GPSINSInput input)
        {
            var inputFromPrev = new StepInput<Matrix>(_prevEstimate);
            StepOutput<Matrix> result = CalculateNext(inputFromPrev, obs, input);
            _prevEstimate = result;

            return ToFilterResult(result);
        }
        public void Update(SimulationStepResults trueState, long elapsedMillis, long totalElapsedMillis,
                           JoystickOutput currentOutput)
        {
            #if XBOX
            // TODO Xbox equivalent or fix the GPSINSFilter dependency on Iridium Math.Net
            _isInitialized = true;
            #else

            // TODO If sensors are not ready we cannot produce an estimated state
            // However, currently the helicopter starts mid-air so we have no time to wait for it to initialize.. so this is cheating.
            // When the helicopter starts on the ground we can properly implement this process.
            if (!_isInitialized)
            {
                if (!_sensors.Ready) return;

                _estimated.Position = _initialState.Position;
                _estimated.Velocity = _initialState.Velocity;
                _estimated.Acceleration = _initialState.Acceleration;
                _estimated.Orientation = _initialState.Orientation;
                _prevGPSPos = _sensors.GPS.Output.Position;

                _gpsins = new GPSINSFilter(
                    TimeSpan.FromMilliseconds(totalElapsedMillis - elapsedMillis),
                    _initialState.Position,
                    _initialState.Velocity,
                    _initialState.Orientation,
                    _sensors.Specifications);

                _isInitialized = true;
            }

            TimeSpan totalTime = TimeSpan.FromMilliseconds(totalElapsedMillis);

            // Setup observations
            var observations = new GPSINSObservation();
            observations.Time = totalTime;
            observations.RangeFinderHeightOverGround = _sensors.GroundRangeFinder.FlatGroundHeight; // TODO Update frequency? Noise?

            if (_sensors.GPS.IsUpdated)
            {
                observations.GotGPSUpdate = true;
                observations.GPSPosition = _sensors.GPS.Output.Position;
                observations.GPSHVelocity = _sensors.GPS.Output.Velocity;
            }

            // Setup input to filter's internal model
            var input = new GPSINSInput
                            {
                                AccelerationWorld = _sensors.IMU.AccelerationWorld,
                                Orientation = _sensors.IMU.AccumulatedOrientation,
            //                                PitchDelta = _sensors.IMU.AngularDeltaBody.Radians.Pitch,
            //                                RollDelta = _sensors.IMU.AngularDeltaBody.Radians.Roll,
            //                                YawDelta = _sensors.IMU.AngularDeltaBody.Radians.Yaw,
            //                                PitchRate = _sensors.IMU.Output.AngularRate.Radians.Pitch,
            //                                RollRate = _sensors.IMU.Output.AngularRate.Radians.Roll,
            //                                YawRate = _sensors.IMU.Output.AngularRate.Radians.Yaw,
                            };

            // Estimate
            StepOutput<GPSINSOutput> gpsinsEstimate = _gpsins.Filter(observations, input);

            //            Vector3 deltaPosition = gpsinsEstimate.PostX.Position - _currentEstimate.Position;
            //            if (deltaPosition.Length() > 40)
            //                deltaPosition = deltaPosition;

            //            var trueState = CheatingTrueState.Result;
            GPSINSOutput observed = gpsinsEstimate.Z;
            GPSINSOutput estimated = gpsinsEstimate.PostX;
            GPSINSOutput blindEstimated = gpsinsEstimate.X;

            _currentEstimate = new PhysicalHeliState(estimated.Orientation, estimated.Position, estimated.Velocity,
                                                     input.AccelerationWorld);

            _currentBlindEstimate = new PhysicalHeliState(blindEstimated.Orientation, blindEstimated.Position, blindEstimated.Velocity,
                                                     input.AccelerationWorld);

            _currentObservation = new PhysicalHeliState(observed.Orientation, observed.Position, observed.Velocity,
                                                        Vector3.Zero);
            #endif
        }
        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,
            });
        }