Exemplo n.º 1
0
    /*************************************************************************************************
    * Public functions & standard Unity callbacks                                                   *
    *************************************************************************************************/

    // Use this for initialization
    void Start()
    {
        if (p != null && p.id == 1)
        {
            kalman = new Kalman(new Vector4(player.transform.position.x, 0, player.transform.position.y, 0),
                                1, 1);
        }
        else
        {
            kalman = new Kalman(new Vector4(player.transform.position.x, 0, player.transform.position.y, 0),
                                0.1, 0.1);
        }
        posHistory         = new Vector2[60];
        posHistoryIterator = 0;
        stateTimer         = Time.time;
        IAState            = (p != null && p.id == 0) ? SWIAState.RANDOM : SWIAState.DODGE_ATTACKS;
        actionNumber       = 0;


//#if UNITY_EDITOR
//#else
        target.SetActive(false);
        noisedPosition.SetActive(false);
        imprecision.SetActive(false);
//#endif
    }
Exemplo n.º 2
0
        public MCvPoint3D32f Kalman4Joints(float x, float y, float z, Kalman kalman, SyntheticData syntheticData)
        {
            //Kalman headPC1
            Emgu.CV.Matrix <float> MatrixGet = new Emgu.CV.Matrix <float>(6, 1);
            MatrixGet[0, 0] = x;
            MatrixGet[1, 0] = y;
            MatrixGet[2, 0] = z;

            kalman = new Kalman(6, 3, 0);

            Emgu.CV.Matrix <float> state = MatrixGet;
            kalman.CorrectedState             = state;
            kalman.TransitionMatrix           = syntheticData.transitionMatrix;
            kalman.MeasurementNoiseCovariance = syntheticData.measurementNoise;
            kalman.ProcessNoiseCovariance     = syntheticData.processNoise;
            kalman.ErrorCovariancePost        = syntheticData.errorCovariancePost;
            kalman.MeasurementMatrix          = syntheticData.measurementMatrix;

            Matrix <float> prediction = new Matrix <float>(3, 1);

            prediction = kalman.Predict();
            MCvPoint3D32f predictPointheadPC1 = new MCvPoint3D32f(prediction[0, 0], prediction[1, 0], prediction[2, 0]);
            MCvPoint3D32f measurePointheadPC1 = new MCvPoint3D32f(syntheticData.GetMeasurement()[0, 0],
                                                                  syntheticData.GetMeasurement()[1, 0], syntheticData.GetMeasurement()[2, 0]);
            Matrix <float> estimated      = kalman.Correct(syntheticData.GetMeasurement());
            MCvPoint3D32f  estimatedPoint = new MCvPoint3D32f(estimated[0, 0], estimated[1, 0], estimated[2, 0]);

            syntheticData.GoToNextState();

            return(estimatedPoint);
        }
Exemplo n.º 3
0
        void Start()
        {
            // Since output rotation is flipped(rotated 180), we have to rotate the model.
            // Calculate the model Rotate in advance to save computation
            iR = Quaternion.AngleAxis(180, Vector3.forward) * this.transform.localRotation;

            this.transform.localPosition = new Vector3(0, 0, -30);
            // Position adjustment for each platform
#if UNITY_STANDALONE
            correction = Matrix4x4.Scale(new Vector3(6.5f, 6.2f, 9.3f));
            // this.transform.localScale = Vector3.one * 30;
            this.transform.localScale = this.transform.localScale * 30;
#elif UNITY_IPHONE
            correction = Matrix4x4.Scale(new Vector3(15.2f, 15.2f, 9.1f));
            this.transform.localScale = this.transform.localScale * 30;
#elif UNITY_ANDROID
            correction = Matrix4x4.Scale(new Vector3(8.1f, 8.1f, 9.1f));
            this.transform.localScale = this.transform.localScale * 46;
#else
            correction = Matrix4x4.Scale(new Vector3(6.5f, 6.2f, 9.3f));
            this.transform.localScale = this.transform.localScale * 32;
#endif
            // Acquire renderer for blendshape
            smr = this.GetComponentInChildren <SkinnedMeshRenderer>();
            if (smr == null)
            {
                throw new UnityException("SkinnedMeshRenderer for this model is missing");
            }

            filter = new Kalman(kalmanRatio);
        }
Exemplo n.º 4
0
 void Awake()
 {
     KalmanPitch   = new Kalman();
     KalmanRoll    = new Kalman();
     dataCollector = new DataCollector();
     dataCollector.Start();
 }
Exemplo n.º 5
0
        //Setup Kalman Filter and predict methods
        public void InitializeKalmanFilter()
        {
            _kalmanXYZ = new Kalman(6, 3, 0);
            Matrix <float> state = new Matrix <float>(new float[]
            {
                0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f
            });

            _kalmanXYZ.CorrectedState   = state;
            _kalmanXYZ.TransitionMatrix = new Matrix <float>(new float[, ]
            {
                // x-pos, y-pos, z-pos, x-velocity, y-velocity, z-velocity
                { 1, 0, 0, 1, 0, 0 },
                { 0, 1, 0, 0, 1, 0 },
                { 0, 0, 1, 0, 0, 1 },
                { 0, 0, 0, 1, 0, 0 },
                { 0, 0, 0, 0, 1, 0 },
                { 0, 0, 0, 0, 0, 1 }
            });
            _kalmanXYZ.MeasurementNoiseCovariance = new Matrix <float>(3, 3); //Fixed accordiong to input data
            _kalmanXYZ.MeasurementNoiseCovariance.SetIdentity(new MCvScalar(1.0e-1));

            _kalmanXYZ.ProcessNoiseCovariance = new Matrix <float>(6, 6);         //Linked to the size of the transition matrix
            _kalmanXYZ.ProcessNoiseCovariance.SetIdentity(new MCvScalar(1.0e-4)); //The smaller the value the more resistance to noise

            _kalmanXYZ.ErrorCovariancePost = new Matrix <float>(6, 6);            //Linked to the size of the transition matrix
            _kalmanXYZ.ErrorCovariancePost.SetIdentity();
            _kalmanXYZ.MeasurementMatrix = new Matrix <float>(new float[, ]
            {
                { 1, 0, 0, 0, 0, 0 },
                { 0, 1, 0, 0, 0, 0 },
                { 0, 0, 1, 0, 0, 0 }
            });
        }
Exemplo n.º 6
0
 public Optimization(int[] n, double s, int d)
 {
     this.kalman = new Kalman(Variable.getVariance(), Variable.getPredeterminedVariance(), Average(n));
     this.K      = new double[n.Length];
     this.R      = n;
     this.SideS  = s;
     this.D      = d;
     for (int i = 0; i < R.Length; i++)
     {
         K[1] = R[i];
     }
 }
Exemplo n.º 7
0
        public Matrix <float> FilterPoints(Kalman kalman, float x, float y, float z)
        {
            Matrix <float> prediction = kalman.Predict();
            Matrix <float> estimated  = kalman.Correct(new Matrix <float>(new[] { x, y, z }));
            Matrix <float> results    = new Matrix <float>(new[, ]
            {
                { prediction[0, 0], prediction[1, 0], prediction[2, 0] },
                { estimated[0, 0], estimated[1, 0], estimated[2, 0] }
            });

            return(results);
        }
Exemplo n.º 8
0
        private void inicializarKalman()
        {
            kal = new Kalman(4, 2, 0);
            kal.TransitionMatrix = new Matrix <float>(new float[4, 4] {
                { 1, 0, 1, 0 }, { 0, 1, 0, 1 }, { 0, 0, 1, 0 }, { 0, 0, 0, 1 }
            });

            kal.MeasurementMatrix.SetIdentity();
            kal.ProcessNoiseCovariance.SetIdentity(new MCvScalar(1e-4));
            kal.MeasurementNoiseCovariance.SetIdentity(new MCvScalar(1e-4));
            kal.ErrorCovariancePost.SetIdentity(new MCvScalar(1e-4));

            medidaKalman = new Matrix <float>(2, 1);
        }
Exemplo n.º 9
0
        public override void UpdateState(Vector3 point)
        {
            componentCount = Math.Max(1, componentCount);

            if (Kalman != null &&
                componentCount == ComponentCount &&
                Mathf.Approximately(measurementNoise, mNoise) &&
                processNoiseScale == procNoiseK &&
                Mathf.Approximately(processNoiseExponent, procNoiseE))
            {
                Kalman.Correct(point);
            }
            else
            {
                ComponentCount = componentCount;
                Dimension      = VectorSize * ComponentCount;
                mNoise         = measurementNoise;
                procNoiseK     = processNoiseScale;
                procNoiseE     = processNoiseExponent;

                var initialState = new Vector3[ComponentCount];
                initialState[0] = lastPoint ?? point;

                Kalman = new DiscreteKalmanFilter <Vector3[], Vector3>(
                    initialState,
                    GetProcessNoise(_ => processNoiseScale),
                    (int)VectorSize,
                    0,
                    ToArray,
                    ToVector3s,
                    ToArray)
                {
                    ProcessNoise = GetProcessNoise(i =>
                                                   Mathf.Pow(processNoiseScale, processNoiseExponent * (i + 1))),
                    MeasurementMatrix = MakePositionMeasurementMatrix(),
                    TransitionMatrix  = MakeTransitionMatrix(),
                    MeasurementNoise  = Matrix.Diagonal((int)VectorSize, (double)measurementNoise)
                };

                if (lastPoint != null)
                {
                    Kalman.Correct(point);
                }
                Kalman.Predict();
                lastPoint = point;
            }
        }
Exemplo n.º 10
0
 public void KalmanFilter()
 {
     mousePoints = new List<PointF>();
         kalmanPoints = new List<PointF>();
         kal = new Kalman(4, 2, 0);
         syntheticData = new SyntheticData();
         Matrix<float> state = new Matrix<float>(new float[]
         {
             0.0f, 0.0f, 0.0f, 0.0f
         });
         kal.CorrectedState = state;
         kal.TransitionMatrix = syntheticData.transitionMatrix;
         kal.MeasurementNoiseCovariance = syntheticData.measurementNoise;
         kal.ProcessNoiseCovariance = syntheticData.processNoise;
         kal.ErrorCovariancePost = syntheticData.errorCovariancePost;
         kal.MeasurementMatrix = syntheticData.measurementMatrix;
 }
Exemplo n.º 11
0
    public KalmanTracker()
    {
        kalmanPoints  = new List <PointF>();
        kal           = new Kalman(4, 2, 0);
        syntheticData = new Kalman_Filter.SyntheticData();
        Matrix <float> state = new Matrix <float>(new float[]
        {
            0.0f, 0.0f, 0.0f, 0.0f
        });

        kal.CorrectedState             = state;
        kal.TransitionMatrix           = syntheticData.transitionMatrix;
        kal.MeasurementNoiseCovariance = syntheticData.measurementNoise;
        kal.ProcessNoiseCovariance     = syntheticData.processNoise;
        kal.ErrorCovariancePost        = syntheticData.errorCovariancePost;
        kal.MeasurementMatrix          = syntheticData.measurementMatrix;
    }
Exemplo n.º 12
0
 /// <summary>
 /// Resets the AI to be ready for a new round
 /// </summary>
 public override void onRestart()
 {
     base.onRestart();
     if (p != null && p.id == 1)
     {
         kalman = new Kalman(new Vector4(player.transform.position.x, 0, player.transform.position.y, 0),
                             1, 1);
     }
     else
     {
         kalman = new Kalman(new Vector4(player.transform.position.x, 0, player.transform.position.y, 0),
                             0.1, 0.1);
     }
     posHistory         = new Vector2[60];
     posHistoryIterator = 0;
     stateTimer         = Time.time;
     IAState            = (p != null && p.id == 0) ? SWIAState.RANDOM : SWIAState.DODGE_ATTACKS;
     IAAction           = (p != null && p.id == 0) ? SWIAAction.MOVE_TURN_LEFT : SWIAAction.MOVE_FORWARD;
     actionNumber       = 0;
 }
        public KalmanFilterTrack()
        {
            //initialize new kalman filter with appropriate number of parameters
            kal = new Kalman(6, 3, 0);
            mk  = new ModelKalman();
            Matrix <float> state = new Matrix <float>(new float[]
            {
                0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f
            });

            //define kalman filter according with the kalman model
            kal.CorrectedState             = state;
            kal.TransitionMatrix           = mk.transitionMatrix;
            kal.MeasurementNoiseCovariance = mk.measurementNoise;
            kal.ProcessNoiseCovariance     = mk.processNoise;
            kal.ErrorCovariancePost        = mk.errorCovariancePost;
            kal.MeasurementMatrix          = mk.measurementMatrix;
            start = true;
            pars  = new float[3];
        }
        //手势区域卡尔曼滤波初始化
        public MyKalman(int m, int n)
        {
            this.m = m;
            this.n = n;
            kalman = new Kalman(m, n, 0);

            if (m == 6)
            {
                InitHand();
            }
            else
            {
                InitGesture();
            }

            kalman.CorrectedState             = state;
            kalman.TransitionMatrix           = this.transitionMatrix;
            kalman.MeasurementNoiseCovariance = this.measurementNoise;
            kalman.ProcessNoiseCovariance     = this.processNoise;
            kalman.ErrorCovariancePost        = this.errorCovariancePost;
            kalman.MeasurementMatrix          = this.measurementMatrix;
        }
Exemplo n.º 15
0
        public Ball()
        {
            m_PreviousBallPositions = new List <PointF>();

            m_KalmanFilter = new Kalman(4, 2, 0);
            m_KalmanFilter.CorrectedState   = new Matrix <float>(new float[] { 0f, 0f, 0f, 0f });
            m_KalmanFilter.TransitionMatrix = new Matrix <float>(new float[, ] {
                { 1f, 0, 1, 0 }, { 0, 1f, 0, 1 }, { 0, 0, 0.85f, 0 }, { 0, 0, 0, 0.85f }
            });
            m_KalmanFilter.MeasurementNoiseCovariance = new Matrix <float>(new float[, ] {
                { 0.0006f, 0 }, { 0, 0.0006f }
            });
            m_KalmanFilter.ProcessNoiseCovariance = new Matrix <float>(new float[, ] {
                { 0.1f, 0, 0, 0 }, { 0, 0.1f, 0, 0 }, { 0, 0, 0.1f, 0 }, { 0, 0, 0, 0.1f }
            });
            m_KalmanFilter.ErrorCovariancePost = new Matrix <float>(new float[, ] {
                { 1f, 0, 0, 0 }, { 0, 1f, 0, 0 }, { 0, 0, 1f, 0 }, { 0, 0, 0, 1f }
            });
            m_KalmanFilter.MeasurementMatrix = new Matrix <float>(new float[, ] {
                { 1f, 0, 0, 0 }, { 0, 1f, 0, 0 }
            });
        }
Exemplo n.º 16
0
        public KalmanFilterTrack(bool use, bool twoflies, Photodiode pd)
        {
            //initialize new kalman filter with appropriate number of parameters
            kal = new Kalman(6, 3, 0);
            mk  = new ModelKalman();
            Matrix <float> state = new Matrix <float>(new float[]
            {
                0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f
            });

            //define kalman filter according with the kalman model
            kal.CorrectedState             = state;
            kal.TransitionMatrix           = mk.transitionMatrix;
            kal.MeasurementNoiseCovariance = mk.measurementNoise;
            kal.ProcessNoiseCovariance     = mk.processNoise;
            kal.ErrorCovariancePost        = mk.errorCovariancePost;
            kal.MeasurementMatrix          = mk.measurementMatrix;
            pars          = new float[3];
            this.use      = use;
            this.twoflies = twoflies;

            photodiode = pd;
        }
Exemplo n.º 17
0
 public void KalmanFilter(int camID)
 {
     if (camID == 0)
     {
         mousePoints   = new List <PointF>();
         kalmanPoints  = new List <PointF>();
         kal           = new Kalman(4, 2, 0);
         syntheticData = new SyntheticData();
         Matrix <float> state = new Matrix <float>(new float[]
         {
             0.0f, 0.0f, 0.0f, 0.0f
         });
         kal.CorrectedState             = state;
         kal.TransitionMatrix           = syntheticData.transitionMatrix;
         kal.MeasurementNoiseCovariance = syntheticData.measurementNoise;
         kal.ProcessNoiseCovariance     = syntheticData.processNoise;
         kal.ErrorCovariancePost        = syntheticData.errorCovariancePost;
         kal.MeasurementMatrix          = syntheticData.measurementMatrix;
     }
     else if (camID == 1)
     {
         mousePoints2   = new List <PointF>();
         kalmanPoints2  = new List <PointF>();
         kal2           = new Kalman(4, 2, 0);
         syntheticData2 = new SyntheticData();
         Matrix <float> state2 = new Matrix <float>(new float[]
         {
             0.0f, 0.0f, 0.0f, 0.0f
         });
         kal2.CorrectedState             = state2;
         kal2.TransitionMatrix           = syntheticData2.transitionMatrix;
         kal2.MeasurementNoiseCovariance = syntheticData2.measurementNoise;
         kal2.ProcessNoiseCovariance     = syntheticData2.processNoise;
         kal2.ErrorCovariancePost        = syntheticData2.errorCovariancePost;
         kal2.MeasurementMatrix          = syntheticData2.measurementMatrix;
     }
 }
Exemplo n.º 18
0
        public void TestKalman()
        {
            Image <Bgr, Byte> img = new Image <Bgr, byte>(400, 400);

            SyntheticData syntheticData = new SyntheticData();

            // state is (phi, delta_phi) - angle and angle increment
            Matrix <float> state = new Matrix <float>(new float[] { 0.0f, 0.0f }); //initial guess

            #region initialize Kalman filter
            Kalman tracker = new Kalman(2, 1, 0);
            tracker.TransitionMatrix           = syntheticData.TransitionMatrix;
            tracker.MeasurementMatrix          = syntheticData.MeasurementMatrix;
            tracker.ProcessNoiseCovariance     = syntheticData.ProcessNoise;
            tracker.MeasurementNoiseCovariance = syntheticData.MeasurementNoise;
            tracker.ErrorCovariancePost        = syntheticData.ErrorCovariancePost;
            tracker.CorrectedState             = state;
            #endregion

            System.Converter <double, PointF> angleToPoint =
                delegate(double radianAngle)
            {
                return(new PointF(
                           (float)(img.Width / 2 + img.Width / 3 * Math.Cos(radianAngle)),
                           (float)(img.Height / 2 - img.Width / 3 * Math.Sin(radianAngle))));
            };

            Action <PointF, Bgr> drawCross =
                delegate(PointF point, Bgr color)
            {
                img.Draw(new Cross2DF(point, 15, 15), color, 1);
            };

            ImageViewer viewer = new ImageViewer();
            System.Windows.Forms.Timer timer = new System.Windows.Forms.Timer();
            timer.Interval = 200;
            timer.Tick    += new EventHandler(delegate(object sender, EventArgs e)
            {
                Matrix <float> measurement = syntheticData.GetMeasurement();
                // adjust Kalman filter state
                tracker.Correct(measurement);

                tracker.Predict();

                #region draw the state, prediction and the measurement
                PointF statePoint       = angleToPoint(tracker.CorrectedState[0, 0]);
                PointF predictPoint     = angleToPoint(tracker.PredictedState[0, 0]);
                PointF measurementPoint = angleToPoint(measurement[0, 0]);

                img.SetZero();                                                                     //clear the image
                drawCross(statePoint, new Bgr(Color.White));                                       //draw current state in White
                drawCross(measurementPoint, new Bgr(Color.Red));                                   //draw the measurement in Red
                drawCross(predictPoint, new Bgr(Color.Green));                                     //draw the prediction (the next state) in green
                img.Draw(new LineSegment2DF(statePoint, predictPoint), new Bgr(Color.Magenta), 1); //Draw a line between the current position and prediction of next position

                //Trace.WriteLine(String.Format("Velocity: {0}", tracker.CorrectedState[1, 0]));
                #endregion

                syntheticData.GoToNextState();

                viewer.Image = img;
            });
            timer.Start();
            viewer.Disposed += delegate(Object sender, EventArgs e) { timer.Stop(); };
            viewer.Text      = "Actual State: White; Measurement: Red; Prediction: Green";
            viewer.ShowDialog();
        }
Exemplo n.º 19
0
        // Get the GPS data from the file
        public static GPSDataList ReadGPS(string _file, GPSBox _gpsBox)
        {
            GPSDataList GPSDataList = new GPSDataList();

            try
            {
                // Open the file in Read mode
                FileStream fin = new FileStream(_file, FileMode.Open, FileAccess.Read, FileShare.ReadWrite);

                // Seek to the end of the "GPS " boc
                // Skip the first 16 'header' bits
                Streams.Seek(fin, _gpsBox.Pos + 16);
                int gps_read = 16;

                // While we are reading in the sub box
                while (gps_read < _gpsBox.Size - 8)
                {
                    // Get the GPS box position and size
                    int gps_box_pos  = (int)Streams.ReadUInt(fin, true);
                    int gps_box_size = (int)Streams.ReadUInt(fin, true);

                    // Update our position
                    gps_read = gps_read + 8;

                    // Save the current file seek position
                    long pos = fin.Position;

                    // Seek to this GPS data
                    fin.Seek(gps_box_pos, SeekOrigin.Begin);

                    // Get the GPS box, size, type and magic
                    int    gps_box_size1 = (int)Streams.ReadUInt(fin, true);
                    string gps_box_type  = Streams.ReadString(fin, 4);
                    string gps_box_magic = Streams.ReadString(fin, 4);

                    // Verify the read data is GPS data
                    if (gps_box_size != gps_box_size1 || gps_box_type != "free" || gps_box_magic != "GPS ")
                    {
                        // Log incorrect data format
                        Log.WriteLine("ERROR", "Box at position: " + gps_box_pos + " is not the expected format\r\n\t\t\t\t" +
                                      "(Expected size: " + gps_box_size + ", Actual size: " + gps_box_size1 + ", " +
                                      "Expected type: free, Actual type: " + gps_box_type + ", " +
                                      "Expected magic: GPS , Actual magic: " + gps_box_magic + ")\r\n");
                        continue;
                    }

                    // Extract GPS data
                    int version = (int)Streams.ReadUInt(fin);

                    // Maybe able to identify firmware version with this
                    // Firmware 1.1 and 2.02 = 76
                    // Firmware 2.0 = 15344
                    // Firmware 2.01 - NOT COMPATIBLE, BUG IN FIRMWARE

                    if (version == 76)
                    {
                        // Firmware 1.1 or 2.02
                        fin.Seek(32, SeekOrigin.Current);
                    }
                    else if (version == 15344)
                    {
                        // Firmware 2.0
                        // Nothing to do
                    }
                    else
                    {
                        // Unknown Firmware Version
                    }

                    int hour   = (int)Streams.ReadUInt(fin);
                    int minute = (int)Streams.ReadUInt(fin);
                    int second = (int)Streams.ReadUInt(fin);
                    int year   = (int)Streams.ReadUInt(fin);
                    int month  = (int)Streams.ReadUInt(fin);
                    int day    = (int)Streams.ReadUInt(fin);

                    string active      = Streams.ReadString(fin, 1);
                    string latitude_b  = Streams.ReadString(fin, 1);
                    string longitude_b = Streams.ReadString(fin, 1);
                    string unknown2    = Streams.ReadString(fin, 1);
                    float  latitude    = Streams.ReadFloat(fin);
                    float  longitude   = Streams.ReadFloat(fin);

                    // Check the GPS had a fix
                    if (active == "A")
                    {
                        // Correct the speed during load
                        // 1 knot = 0.514444 m/s
                        float speed = Streams.ReadFloat(fin) * (float)0.514444;

                        DateTime dateTime;

                        if (!Enumerable.Range(1, 31).Contains(day))
                        {
                            // TODO: Calculate day from somewhere
                            // Firmware 1.1 has an issue with day, used to get it from file name?
                        }

                        // Create DateTime object
                        if (year != 0 && month != 0 && day != 0)
                        {
                            dateTime = new DateTime((year + 2000), month, day, hour, minute, second);
                        }
                        else
                        {
                            // Date not valid
                            dateTime = new DateTime(1);
                            continue;
                        }

                        // Change coordinate from DDDmm.mmmm format to traditional lat/long
                        double flatitude  = GPSHelpers.FixCoordinates(latitude_b, latitude);
                        double flongitude = GPSHelpers.FixCoordinates(longitude_b, longitude);

                        // Create new GPSData object
                        GPSData gps = new GPSData();

                        gps.DateTime = dateTime;
                        gps.Speed    = speed;

                        // Kalman filter the GPS data to remove jumps
                        Kalman k = new Kalman(gps.Speed);
                        k.Process(flatitude, flongitude, 9.9f, 1000);
                        gps.Latitude  = k.get_lat();
                        gps.Longitude = k.get_lng();

                        if (lastLat != 0 && lastLong != 0)
                        {
                            gps.Distance = GPSHelpers.Distance(lastLat, lastLong, flatitude, flongitude);
                            gps.Heading  = GPSHelpers.Heading(lastLat, lastLong, flatitude, flongitude);
                        }
                        lastLat  = gps.Latitude;
                        lastLong = gps.Longitude;

                        if (!slowSpeed)
                        {
                            GPSDataList.FilteredGPSData.Add(gps);
                        }
                        GPSDataList.AllGPSData.Add(gps);

                        // Remove a few points when speed is low to stop jagged lines
                        if (speed <= 1)
                        {
                            slowSpeed = true;
                        }
                        else
                        {
                            slowSpeed = false;
                        }
                    }
                    else
                    {
                        Log.WriteVerboseLine("GPS Location is not locked. Trying next block.");
                    }

                    // Return to original file seek
                    fin.Seek(pos, SeekOrigin.Begin);
                }
            }
            catch (Exception e)
            {
                // Unable to open file
                Log.WriteLine("ERROR", "Unable to process file: " + _file);
                Log.WriteLine("ERROR", e.Message.ToString());
            }

            return(GPSDataList);
        }