public void Update(Vector accE, long IMUTimeStamp, Vector gps, long gpsTimeStamp, out Vector feedback)
        {
            feedback = null;

            if (_prevTimeStamp == 0)
            {
                _prevTimeStamp = IMUTimeStamp;
                
                return;
            }

            if(gps != null && gpsTimeStamp < IMUTimeStamp)
            {
                MoveParticles(accE, gpsTimeStamp - _prevTimeStamp);

                feedback = GetCurrentPosition();

                TrimParticles(gps);
                _prevTimeStamp = gpsTimeStamp;
            }

            MoveParticles(accE, IMUTimeStamp - _prevTimeStamp);
            Resample();
            _prevTimeStamp = IMUTimeStamp;
            _prevAccE = new Vector(accE);
        }      
 public Vector GetCurrentPosition()
 {
     Vector ret = new Vector(2);
     ret.X = _particles.Average(p => p.X);
     ret.Y = _particles.Average(p => p.Y);
     return ret;
 }
 public static Vector operator % (Matrix a, Vector b)
 {
     Vector ret = new Vector(3);
     for (int i = 0; i < 3; i++)
         for (int j = 0; j < 3; j++)
             ret[i] += a[i, j] * b[j];
     return ret;
 }
        /// <summary>
        /// Calculate the acceleration of e-frame
        /// </summary>
        /// <param name="acc"></param>
        /// <param name="gyr"></param>
        /// <param name="mag"></param>
        /// <param name="time_diff"></param>
        /// <param name="filter_coef">Fusion coefficient</param>
        /// <param name="shift_num">Find gravity from the mininmal of past acc data</param>
        /// <returns></returns>
        public Vector Calculate(Vector acc, Vector gyr, Vector mag, long curTimeStamp, double filter_coef = 0.99, int shift_num = 500)
        {
            _prevAcceleration.Add(acc);
            if (_prevAcceleration.Count > shift_num)
                _prevAcceleration.RemoveAt(0);
            double minAcc = _prevAcceleration.Min(t => t.GetXYZMagnitude());
            Vector gravity = _prevAcceleration.Find(t => t.GetXYZMagnitude() == minAcc);
            Matrix rotationMatrix = GetRotationMatrix(gravity, mag);
            Vector accMagOrientation = GetOrientation(rotationMatrix);
            
            if (_initState)
            {
                Matrix initMatrix = GetRotationMatrixFromOrientation(accMagOrientation);
                _gyroMatrix = _gyroMatrix % initMatrix;
                _gyroOrientation = GetOrientation(_gyroMatrix);
                _initState = false;
            }

            Vector ret = ComputeAccelerationEarth(acc, gravity);
            Vector deltaVector = new Vector(4);
            if (_prevTimeStamp != 0)
            {
                double dt = (curTimeStamp - _prevTimeStamp) / 1000.0;
                deltaVector = GetRotationVectorFromGyro(gyr, dt / 2.0);
            }
            Matrix deltaMatrix = GetRotationMatrixFromVector(deltaVector);
            _gyroMatrix = _gyroMatrix % deltaMatrix;
            _gyroOrientation = GetOrientation(_gyroMatrix);

            Vector fusedOrientation = new Vector(3);
            for (int i = 0; i < 3; i++)
            {
                if(_gyroOrientation[i] < -0.5 * Math.PI && accMagOrientation[i] > 0)
                {
                    fusedOrientation[i] = filter_coef * (_gyroOrientation[i] + 2 * Math.PI) + (1 - filter_coef) * accMagOrientation[i];
                    if (fusedOrientation[i] > Math.PI)
                        fusedOrientation[i] -= 2 * Math.PI;
                }
                else if(accMagOrientation[i] < -0.5 * Math.PI && _gyroOrientation[i] > 0)
                {
                    fusedOrientation[i] = filter_coef * _gyroOrientation[i] + (1 - filter_coef) * (accMagOrientation[i] + 2 * Math.PI);
                    if (fusedOrientation[i] > Math.PI)
                        fusedOrientation[i] -= 2 * Math.PI;
                }
                else
                {
                    fusedOrientation[i] = filter_coef * _gyroOrientation[i] + (1 - filter_coef) * accMagOrientation[i];
                }
            }
            _gyroMatrix = GetRotationMatrixFromOrientation(fusedOrientation);
            fusedOrientation.Value.CopyTo(_gyroOrientation.Value, 0);

            _prevTimeStamp = curTimeStamp;

            return ret;
        }
 public Matrix(Vector a, Vector b, Vector c) :this()
 {
     for(int i = 0; i < 3; i++)
     {
         Value[0, i] = a[i];
     }
     for (int i = 0; i < 3; i++)
     {
         Value[1, i] = b[i];
     }
     for (int i = 0; i < 3; i++)
     {
         Value[2, i] = c[i];
     }
 }
 public Vector GetEstimatedPosition(long timeEclipse)
 {
     Vector ret = new Vector(2);
     if (_prevAccE == null)
     {
         return ret;
     }
     List<Particle> futureParticles = new List<Particle>();
     foreach(var p in _particles)
     {
         futureParticles.Add(new Particle(p));
     }
     foreach(var p in futureParticles)
     {
         p.Move(_prevAccE, timeEclipse / 1000.0);
     }
     ret.X = futureParticles.Average(p => p.X);
     ret.Y = futureParticles.Average(p => p.Y);
     return ret;
 }
 /// <summary>
 /// Move the particle.
 /// </summary>
 /// <param name="timeEclipse">s</param>
 public void Move(Vector acc, double timeEclipse)
 {
     double magnitude = acc.GetXYMagnitude();
     double theta = ParseAzimuth(acc[3]) / 180.0 * Math.PI + ContinuousUniform.Sample(-ParticleFilter.ANG_EPS, ParticleFilter.ANG_EPS);
     double rand_magitude = magnitude + magnitude * ContinuousUniform.Sample(-ParticleFilter.ACC_EPS, ParticleFilter.ACC_EPS);
     double ax = Math.Cos(theta) * rand_magitude;
     double ay = Math.Sin(theta) * rand_magitude;
     X += (2 * Vx + ax) * timeEclipse / 2.0;
     Y += (2 * Vy + ay) * timeEclipse / 2.0;
     Vx += ax * timeEclipse;
     Vy += ay * timeEclipse;
 }
 public Particle(Vector gps) : this(0, 0, 0, 0, 1.0 / ParticleFilter.MAX_SIZE)
 {
     double theta = ContinuousUniform.Sample(0, 2 * Math.PI);
     double magnitude = Math.Sqrt(ContinuousUniform.Sample(0, 1)) * ParticleFilter.GPS_EPS;
     X = gps.X + magnitude * Math.Cos(theta);
     Y = gps.Y + magnitude * Math.Sin(theta);
 }
 private void MoveParticles(Vector accE, long timeEclipse)
 {
     foreach (var particle in _particles)
     {
         particle.Move(accE, timeEclipse / 1000.0);
     }
 }  
 public Form1() 
 {
     InitializeComponent();
     _acc = new Vector(3);
     _mag = new Vector(3);
     _gyr = new Vector(3);
     _gps = new Vector(2);
     _twd97 = new Vector(2);
     _prevTwdForVelocity = new List<Vector>();
     _prevAvgTwdForVelocity = new List<Vector>();
     _IMUTimeStamp = 0;
     _gpsTimeStamp = 0;
     _offline_imuCounter = 0;
     _offline_gpsCounter = 0;
     _sf = new SensorFusion();
     _pf = new ParticleFilter();
     _prevTwdForCurvature = new List<Vector>();
     _prevAvgTwdForCurvature = new List<Vector>();
     _prevCurvatureDf = new List<double>();
     _prevPFTwd = new List<Vector>();
     _prevPFAvgTwd = new List<Vector>();
     _roadData = new List<Tuple<double, double, double>>();
     _pf.SetLogger(lbInfo);
     switch (DATA_MODE)
     {
         case DataMode.Online:
             ReadRoadData();
             break;
         case DataMode.Offline:
             _offline_accs = File.ReadAllLines(PC_FILE_PATH + "Acc.txt");
             _offline_mags = File.ReadAllLines(PC_FILE_PATH + "Mag.txt");
             _offline_gyrs = File.ReadAllLines(PC_FILE_PATH + "Gyr.txt");
             _offline_gpss = File.ReadAllLines(PC_FILE_PATH + "GPS.txt");
             _offline_roadCurvatures = File.ReadAllLines(PC_FILE_PATH + "GPS3_cur.txt");
             break;
         default:
             throw new InvalidOperationException("You don't set DataMode.");
     }
     
 }
        private void CalculateEstimatedCurvature()
        {
            if (ESTI_MODE == EstimationMode.GPS)
            {
                CalculateGPSCurvature();
            }
            
            if(ESTI_MODE == EstimationMode.ParticleFilter)
            {
                List<Vector> futureTwd = new List<Vector>(_prevPFTwd);
                List<Vector> futureAvgTwd = new List<Vector>(_prevPFAvgTwd);
                List<double> futureCurvatureDf = new List<double>(_prevCurvatureDf);

                if (futureTwd.Count >= PF_POSITION_MOVING_AVERAGE_COUNT)
                    futureTwd.RemoveAt(0);
                futureTwd.Add(new Vector(_predictPosition));

                Vector tmp = new Vector(2);
                tmp.X = futureTwd.Average(twd => twd.X);
                tmp.Y = futureTwd.Average(twd => twd.Y);
                if (futureAvgTwd.Count >= 3)
                    futureAvgTwd.RemoveAt(0);
                futureAvgTwd.Add(tmp);

                if (futureAvgTwd.Count < 3)
                    return;

                int lastIndex = futureAvgTwd.Count;
                Vector a = futureAvgTwd[lastIndex - 3];
                Vector b = futureAvgTwd[lastIndex - 2];
                Vector c = futureAvgTwd[lastIndex - 1];
                _curvature = CalculateCurvature(a, b, c);
                if (futureCurvatureDf.Count >= PF_CURVATURE_MOVING_AVERAGE_COUNT)
                    futureCurvatureDf.RemoveAt(0);
                futureCurvatureDf.Add(_curvature - _roadCurvature);

                _curvatureDf = futureCurvatureDf.Average();
            }            
        }
 private double CalculateCurvature(Vector t0, Vector t1, Vector t2)
 {
     double a = Math.Sqrt(Math.Pow(t0.X - t1.X, 2) + Math.Pow(t0.Y - t1.Y, 2));
     double b = Math.Sqrt(Math.Pow(t1.X - t2.X, 2) + Math.Pow(t1.Y - t2.Y, 2));
     double c = Math.Sqrt(Math.Pow(t0.X - t2.X, 2) + Math.Pow(t0.Y - t2.Y, 2));
     double theta = (a * a + b * b - c * c) / (2 * a * b);
     double curvature = 1.0 / (c / (2 * Math.Sqrt((1 - theta * theta))));
     double x1 = t1.X - t0.X;
     double x2 = t2.X - t1.X;
     double y1 = t1.Y - t0.Y;
     double y2 = t2.Y - t1.Y;
     if (x1 * y2 - x2 * y1 > 0)
         curvature *= -1;
     return curvature;
 }
 private static Vector GetOrientation(Matrix r)
 {
     Vector ret = new Vector(3);
     ret.Value[0] = Math.Atan2(r.Value[0, 1], r.Value[1, 1]);
     ret.Value[1] = Math.Asin(-r.Value[2, 1]);
     ret.Value[2] = Math.Atan2(-r.Value[2, 0], r.Value[2, 2]);
     return ret;
 }
        private void timer1_Tick(object sender, EventArgs e)
        {
            string[] acc = null;
            string[] mag = null;
            string[] gyr = null;
            string[] gps = null;
            string[] road = null;

            switch (DATA_MODE)
            {
                case DataMode.Online:
                    DownloadData();
                    LoadData();
                    acc = _accs.Split(",".ToCharArray());
                    mag = _mags.Split(",".ToCharArray());
                    gyr = _gyrs.Split(",".ToCharArray());
                    gps = _gpss.Split(",".ToCharArray());
                    break;
                case DataMode.Offline:
                    acc = _offline_accs[_offline_imuCounter].Split(",".ToCharArray());
                    mag = _offline_mags[_offline_imuCounter].Split(",".ToCharArray());
                    gyr = _offline_gyrs[_offline_imuCounter].Split(",".ToCharArray());
                    gps = _offline_gpss[_offline_gpsCounter].Split(",".ToCharArray());
                    road = _offline_roadCurvatures[10].Split(",".ToCharArray());
                    break;
            }

            for (int j = 0; j < 3; j++)
            {
                _acc[j] = double.Parse(acc[j + 1]);
                _mag[j] = double.Parse(mag[j + 1]);
                _gyr[j] = double.Parse(gyr[j + 1]);
            }
            _gps[0] = double.Parse(gps[1]);
            _gps[1] = double.Parse(gps[2]);
            _currentIMUTimeStamp = long.Parse(acc[0]);
            _currentGPSTimeStamp = long.Parse(gps[0]);

            if (_startTimeStamp == 0)
                _startTimeStamp = _IMUTimeStamp;

            if (_gpsTimeStamp == 0 || _gpsTimeStamp < _currentGPSTimeStamp)
            {
                double[] ret = GPSConverter.GetTWD97(_gps[0], _gps[1]);
                _twd97.X = ret[0];
                _twd97.Y = ret[1];
                AddTwdForCurvature(_twd97);
                AddTwdForVelocity(_twd97, _currentGPSTimeStamp);
                CalculateVelocity();
                lbInfo.Items.Add("[GPS][" + (_currentGPSTimeStamp - _startTimeStamp) + "]: " + _twd97[0] + ", " + _twd97[1]);
                _gpsTimeStamp = _currentGPSTimeStamp;

                if(DATA_MODE == DataMode.Online)
                {
                    _roadCurvature = QueryRoadData(_twd97.X, _twd97.Y);
                }
                if(DATA_MODE == DataMode.Offline)
                {
                    _roadCurvature = double.Parse(road[3]);
                }
            }

            if (_IMUTimeStamp == 0 || _IMUTimeStamp < _currentIMUTimeStamp)
            {
                if (ESTI_MODE == EstimationMode.GPS)
                {
                    CalculateEstimatedCurvature();
                }

                if (ESTI_MODE == EstimationMode.ParticleFilter)
                {
                    _accE = _sf.Calculate(new Vector(_acc), new Vector(_gyr), new Vector(_mag), _currentIMUTimeStamp);

                    // 使用GPS觀察前的預估位置
                    Vector feedback = null;
                    _pf.Update(_accE, _currentIMUTimeStamp, _twd97, _gpsTimeStamp, out feedback);

                    if(feedback != null)
                    {
                        AddPFTwd(feedback);
                        CalculatePFCurvature();
                    }

                    // Current or Future ?
                    long time = _gpsTimeStamp + 1000 - _currentIMUTimeStamp;
                    //_predictPosition = _pf.GetCurrentPosition();
                    _predictPosition = _pf.GetEstimatedPosition(FUTURE_TIME);
                    CalculateEstimatedCurvature();
                }
                                
                _IMUTimeStamp = _currentIMUTimeStamp;
            }

            if(DATA_MODE == DataMode.Offline)
            {
                if (_IMUTimeStamp > _gpsTimeStamp)
                    _offline_gpsCounter++;
                _offline_imuCounter++;
            }

            ShowResult();
        }
 public Vector(Vector v) : this(v.Length)
 {
     v.Value.CopyTo(Value, 0);
 }
 public static Vector operator - (Vector a, Vector b)
 {
     if (a.Length != b.Length)
         throw new InvalidOperationException();
     Vector ret = new Vector(a.Length);
     for (int i = 0; i < ret.Length; i++)
         ret[i] = a[i] - b[i];
     return ret;
 }
 private Vector ComputeAccelerationEarth(Vector v, Vector gravity)
 {
     Vector ret = new Vector(4);
     Vector tmp = _gyroMatrix % (v - gravity);
     tmp.Value.CopyTo(ret.Value, 0);
     ret.Value[3] = _gyroOrientation.Value[0] * 180.0 / Math.PI;
     return ret;
 }
 private static Matrix GetRotationMatrixFromOrientation(Vector a)
 {
     double sinX = Math.Sin(a.Value[1]);
     double cosX = Math.Cos(a.Value[1]);
     double sinY = Math.Sin(a.Value[2]);
     double cosY = Math.Cos(a.Value[2]);
     double sinZ = Math.Sin(a.Value[0]);
     double cosZ = Math.Cos(a.Value[0]);
     Matrix xM = new Matrix(
         new Vector(1, 0, 0),
         new Vector(0, cosX, sinX),
         new Vector(0, -sinX, cosX));
     Matrix yM = new Matrix(
         new Vector(cosY, 0, sinY),
         new Vector(0, 1, 0),
         new Vector(-sinY, 0, cosY));
     Matrix zM = new Matrix(
         new Vector(cosZ, sinZ, 0),
         new Vector(-sinZ, cosZ, 0),
         new Vector(0, 0, 1));
     return zM % (xM % yM);
 }
 private static Vector GetRotationVectorFromGyro(Vector gyro, double time)
 {
     Vector tmp = new Vector(gyro);
     double magnitude = tmp.GetXYZMagnitude();
     if (magnitude > 0.00000001)
         tmp.Normalization();
     double thetaOverTwo = magnitude * time;
     double sinThetaOverTwo = Math.Sin(thetaOverTwo);
     double cosThetaOverTwo = Math.Cos(thetaOverTwo);
     Vector ret = new Vector(4);
     for (int i = 0; i < 3; i++)
         ret[i] = sinThetaOverTwo * tmp[i];
     ret[3] = cosThetaOverTwo;
     return ret;
 }
 private void AddTwdForCurvature(Vector twd97)
 {
     if (_prevTwdForCurvature.Count >= GPS_CURVATURE_POSITION_MOVING_AVERAGE_COUNT)
         _prevTwdForCurvature.RemoveAt(0);
     _prevTwdForCurvature.Add(new Vector(twd97));
     if (_prevAvgTwdForCurvature.Count >= 3)
         _prevAvgTwdForCurvature.RemoveAt(0);
     Vector avgTwd97 = new Vector(2);
     avgTwd97.X = _prevTwdForCurvature.Average(twd => twd.X);
     avgTwd97.Y = _prevTwdForCurvature.Average(twd => twd.Y);
     _prevAvgTwdForCurvature.Add(avgTwd97);
 }
 private static Matrix GetRotationMatrixFromVector(Vector v)
 {
     double q0 = 0;
     double q1 = v.Value[0];
     double q2 = v.Value[1];
     double q3 = v.Value[2];
     if (v.Length == 4)
     {
         q0 = v.Value[3];
     }
     else
     {
         q0 = 1 - (q1 * q1 + q2 * q2 + q3 * q3);
         if (q0 > 0)
             q0 = Math.Sqrt(q0);
         else
             q0 = 0;
     }
     double sq_q1 = 2 * q1 * q1;
     double sq_q2 = 2 * q2 * q2;
     double sq_q3 = 2 * q3 * q3;
     double q1_q2 = 2 * q1 * q2;
     double q3_q0 = 2 * q3 * q0;
     double q1_q3 = 2 * q1 * q3;
     double q2_q0 = 2 * q2 * q0;
     double q2_q3 = 2 * q2 * q3;
     double q1_q0 = 2 * q1 * q0;
     return new Matrix(
         new Vector(1 - sq_q2 - sq_q3, q1_q2 - q3_q0, q1_q3 + q2_q0),
         new Vector(q1_q2 + q3_q0, 1 - sq_q1 - sq_q3, q2_q3 - q1_q0),
         new Vector(q1_q3 - q2_q0, q2_q3 + q1_q0, 1 - sq_q1 - sq_q2));
 }
 /// <summary>
 /// Remove particles which are outside gps eps and supply particles up to MAX_SIZE.
 /// </summary>
 /// <param name="gps"></param>
 private void TrimParticles(Vector gps)
 {
     List<Particle> _newParticles = new List<Particle>(_particles);
     _newParticles.RemoveAll(t => Math.Pow((t.X - gps.X), 2) + Math.Pow((t.Y - gps.Y), 2) > Math.Pow(GPS_EPS, 2));
     _logger.Items.Add(_newParticles.Count);
     if(_newParticles.Count == 0)
     {
         while (_newParticles.Count < MAX_SIZE)
         {
             _newParticles.Add(new Particle(gps));
         }
         for (int i = 0; i < MAX_SIZE; i++)
         {
             _newParticles[i].Vx = _particles[i].Vx * ATTENUATION;
             _newParticles[i].Vy = _particles[i].Vy * ATTENUATION;
         }
     }
     else
     {
         int remain = _newParticles.Count;
         while(_newParticles.Count < MAX_SIZE)
         {
             int idx = (int)Math.Floor(ContinuousUniform.Sample(0, remain - 1e-9));
             _newParticles.Add(new Particle(_newParticles[idx]));
         }
     }
     _particles = _newParticles;
 }
 private void AddTwdForVelocity(Vector twd97, long timestamp)
 {
     if (_prevTwdForVelocity.Count >= GPS_VELOCITY_POSITION_MOVING_AVERAGE_COUNT)
         _prevTwdForVelocity.RemoveAt(0);
     _prevTwdForVelocity.Add(new Vector(twd97));
     // 計算速度採第一與第三個位置的平均
     if (_prevAvgTwdForVelocity.Count >= GPS_VELOCITY_POSITION_MOVING_AVERAGE_COUNT)
         _prevAvgTwdForVelocity.RemoveAt(0);
     Vector avgTwd97 = new Vector(3);
     avgTwd97.X = _prevTwdForVelocity.Average(twd => twd.X);
     avgTwd97.Y = _prevTwdForVelocity.Average(twd => twd.Y);
     // 時間戳記
     avgTwd97[2] = timestamp;
     _prevAvgTwdForVelocity.Add(avgTwd97);
 }
 private void AddPFTwd(Vector twd97)
 {
     if (_prevPFTwd.Count >= PF_POSITION_MOVING_AVERAGE_COUNT)
         _prevPFTwd.RemoveAt(0);
     _prevPFTwd.Add(new Vector(twd97));
     if (_prevPFTwd.Count >= 3)
         _prevPFAvgTwd.RemoveAt(0);
     Vector avgTwd = new Vector(2);
     avgTwd.X = _prevPFTwd.Average(twd => twd.X);
     avgTwd.Y = _prevPFTwd.Average(twd => twd.Y);
     _prevPFAvgTwd.Add(avgTwd);
 }
 private static Matrix GetRotationMatrix(Vector gravity, Vector geomanetic)
 {
     Vector A = new Vector(gravity);
     Vector E = new Vector(geomanetic);
     Vector H = E % A;
     double normH = H.GetXYZMagnitude();
     if (normH < 0.1)
         return null;
     H.Normalization();
     A.Normalization();
     Vector M = A % H;
     Matrix ret = new Matrix(H, M, A);
     return ret;
 }