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