private StepOutput <float> CalculateNext(StepInput <float> prev)
        {
            float w = _rand.NextGaussian(0, _stdDevW);
            float v = _rand.NextGaussian(0, _stdDevV);

            // Calculate the state and the output
            float x = _a * prev.X + w;
            float z = _h * x + v;

            // Predictor equations
            float priX     = _a * prev.PostX;
            float residual = z - _h * priX;
            float priP     = _a * _a * prev.PostP + _q;

            // Corrector equations
            float k     = _h * priP / (_h * _h * priP + _r);
            float postP = priP * (1 - _h * k);
            float postX = priX + k * residual;

            return(new StepOutput <float>
            {
                K = k,
                PriX = priX,
                PriP = priP,
                PostX = postX,
                PostP = postP,
                PostXError = postX - x,
                PriXError = priX - x,
                X = x,
                Z = z,
                W = w,
                V = v,
            });
        }
        public StepOutput <GPSFilter2DSample> Filter(GPSObservation obs, GPSFilter2DInput input)
        {
            var inputFromPrev          = new StepInput <Matrix>(_prevEstimate);
            StepOutput <Matrix> result = CalculateNext(inputFromPrev, obs, input);

            _prevEstimate = result;

            return(ToFilterResult(result));
        }
        // Observations are never used.
//        public IEnumerable<StepOutput<GPSFilter2DSample>> Filter(IEnumerable<GPSObservation> samples, IEnumerable<GPSFilter2DInput> inputs)
//        {
//            return samples.Select(s => Filter(s, i));
//        }

        private StepOutput <Matrix> CalculateNext(StepInput <Matrix> prev, GPSObservation observation,
                                                  GPSFilter2DInput input)
        {
            TimeSpan time      = observation.Time;
            TimeSpan timeDelta = time - _prevEstimate.Time;

            UpdateMatrices(time, timeDelta);

            Matrix u = ToInputMatrix(input);

            // 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;
            Matrix z = H * x + v; //ToOutputMatrix(observation) + v;//H * x + v;

            // Predictor equations
            Matrix PriX         = A * prev.PostX + B * u;  // n by 1
            Matrix AT           = Matrix.Transpose(A);
            Matrix PriP         = A * prev.PostP * AT + Q; // n by n
            Matrix residual     = z - H * PriX;
            Matrix residualP    = H * PriP * Matrix.Transpose(H) + R;
            Matrix residualPInv = residualP.Inverse();

            // Corrector equations
            Matrix K = PriP * Matrix.Transpose(H) * residualPInv; // n by m

            // TODO Temp, experimenting with skipping measurements
            //            compileerror
            //            k[PositionY, PositionY] = 1.0;
            //            k[VelocityY, VelocityY] = 1.0;
            //            k[AccelerationY, AccelerationY] = 1.0;


            Matrix PostX = PriX + K * residual; // n by 1
            Matrix PostP = (I - K * H) * PriP;  // n by n


            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,
            });
        }
        /// <summary></summary>
        /// <param name="x0">Initial system state</param>
        /// <param name="postX0">Initial guess of system state</param>
        /// <param name="postP0">Initial guess of a posteriori covariance</param>
        /// <returns></returns>
        private StepOutput <Matrix> GetInitialEstimate(Matrix x0, Matrix postX0, Matrix postP0)
        {
            var startingCondition = new StepInput <Matrix>(x0, postX0, postP0);

            // TODO Is it ok to use an observation for the initial state in this manner?
            var observation = new GPSINSObservation
            {
                GPSPosition = GetPosition(x0),
                Time        = _startTime
            };

            return(CalculateNext(startingCondition, observation, new GPSINSInput()));
        }
        /// <summary></summary>
        /// <param name="x0">Initial system state</param>
        /// <param name="postX0">
        ///   Initial guess of system state
        /// </param>
        /// <param name="postP0">
        ///   Initial guess of a posteriori covariance
        /// </param>
        /// <returns></returns>
        private StepOutput <float> InitializeFirstStep(float x0, float postX0, float postP0)
        {
            var startingCondition = new StepInput <float>(x0, postX0, postP0);

            return(CalculateNext(startingCondition));
        }
        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,
            });
        }