|
|
///////////////////////////////////////////////////////////////////////////////
//
// GPS_Path.cs
//
// By Philip R. Braica (HoshiKata@aol.com, VeryMadSci@gmail.com)
//
// Distributed under the The Code Project Open License (CPOL)
// http://www.codeproject.com/info/cpol10.aspx
///////////////////////////////////////////////////////////////////////////////
namespace KalmanDemo
{
using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
/// <summary>
/// Create a GPS path and emulate a GPS measurement set for that path.
/// Designed to be subclassed.
/// Everything in meters or meters per second.
/// </summary>
public class GPS_Path
{
// Our actual path will be moving in a squared off sort of motion.
// So there are 2 ideals, the "front" and the "back" of the vehicle.
// That is used for steering, so we can "average" to get a mild
// accuracy improvement. For "real" GPS the solution jumps a bit
// as satellites are added / removed from the solution constellation,
// and for the cases where this didn't happen at the same time on
// both GPS systems, we could also use this to detect that situation,
// but we'd also have to deal with them acting lock step, they
// will get in that rut at times, usually depending on
// antenna choice, near by metal, and placement.
public List<double> Ideal_Time = new List<double>();
public List<double> Ideal_X = new List<double>();
public List<double> Ideal_Y = new List<double>();
// Measured things.
public List<double> Meas_Time = new List<double>();
public List<double> Meas_X = new List<double>();
public List<double> Meas_Y = new List<double>();
public List<double> Meas_VX = new List<double>();
public List<double> Meas_VY = new List<double>();
public List<double> Meas_AX = new List<double>();
public List<double> Meas_AY = new List<double>();
/// <summary>
/// 1 sigma positional error.
/// 2 = Copernicus (1 Hz)
/// Everything else is 2.5 (2-10Hz)
/// </summary>
public double GPS_Position1Sigma = 2.5;
/// <summary>
/// 1 sigma velocity error.
/// Copernicus is 0.6
/// Everything else is 0.1
/// </summary>
public double GPS_Velocity1Sigma = 0.1;
/// <summary>
/// 1 sigma acceleration error.
/// Off of a IMU, http://www.sparkfun.com/datasheets/Sensors/Accelerometer/LIS331HH.pdf
/// 3 mg sensitivity when setup for +/-6g (3 axis)
/// 650 micro g/ sqrt(Hz) accuracy, 0.5 to 1kHz.
/// 0.02943 m/s^2 sensititvity, but we're also going to average at 10Hz,
/// 0.0020 m/2^2 noise, so treat as 0.002,
///
/// The bosch: http://dlnmh9ip6v2uc.cloudfront.net/datasheets/Sensors/Accelerometers/BST-BMA180-DS000-07_2.pdf
/// is 0.25 mg w/ 150 micro g/sqrt(Hz) or 0.0024525 m/s^2 sensitivity w/ 0.00046 m/s^2 noise.
///
/// Recommend using something embedded to integrate and thus do much better than the sensitivity,
/// and perhaps run at 100 Hz to 1 kHz, in that scenario, 0.000145 for the Bosch!
///
/// Note for 0.002 get 3.5 cm error (rms)
/// For 0.000145 get 2.5 cm error (rms)
/// </summary>
public double Accelerometer_1Sigma = 0.000145;
/// <summary>
/// Velocity measurement in Hz.
/// </summary>
public double GPS_MeasurementHz = 10;
/// <summary>
/// Amount the second one is in front.
/// </summary>
public double AmountInFront = 1.5;
public int RandomSeed = 0;
/// <summary>
/// Amount of over sampling.
/// </summary>
protected int positionOverSampling = 20;
/// <summary>
/// Constructor.
/// </summary>
public GPS_Path()
{
createPath();
simulateGPS();
}
/// <summary>
/// Constructor.
/// </summary>
public GPS_Path(int seed)
{
RandomSeed = seed;
createPath();
simulateGPS();
}
/// <summary>
/// Compute the positional error, as tangental distance to the
/// path, note this isn't perfect but it should be decent
/// if the points are close enough together.
/// </summary>
/// <param name="x">x value to test</param>
/// <param name="y">y value to test</param>
/// <param name="hint">Start path search.</param>
/// <returns>Error in units of x, y (meters).</returns>
public double ComputeError(double x, double y, ref int hint)
{
int minIndex = 0;
double minErr = -1;
// Find the first point.
List<double> errs = new List<double>();
double dx = 0;
double dy = 0;
hint = hint - 20 * positionOverSampling < 0 ? 0 : hint;
hint = hint < Ideal_X.Count - 10 ? hint : Ideal_X.Count - 10;
int offset = hint;
for (int i = hint; (i < Ideal_X.Count) && (i < hint + 50 * positionOverSampling); i++)
{
dx = Ideal_X[i] - x;
dy = Ideal_Y[i] - y;
double err = (dx * dx) + (dy * dy);
minErr = (minErr < 0) ? err : minErr;
minIndex = err <= minErr ? i - offset : minIndex;
errs.Add(err);
}
int nextIndex = minIndex;
if (minIndex == 0)
{
nextIndex = 1;
}
else
{
if (minIndex == errs.Count - 1)
{
nextIndex = errs.Count - 2;
}
else
{
nextIndex = errs[nextIndex - 1] < errs[nextIndex + 1] ? nextIndex - 1 : nextIndex + 1;
}
}
// Two points are minIndex and nextIndex.
// The triangle has 3 points:
// min, next, (x, y).
// errs[min] = |(x,y) - min|^2.
// errs[next] = |(x,y) - next|^2.
dx = Ideal_X[minIndex] - Ideal_X[nextIndex];
dy = Ideal_Y[minIndex] - Ideal_Y[nextIndex];
double delta = (dx * dx) + (dy * dy);
// Now have all 3 sides.
// Law of cosines.
// (x,y)
// errs(min) / | \ errs(next)
// min ----------- next
// delta
// errs(next)/sin(min) = 2R
// R = abc / sqrt(...)
// We need errs(min)*sin(min) = errs(min)*errs(next)/2R
double a = System.Math.Sqrt(delta);
double b = System.Math.Sqrt(errs[minIndex]);
double c = System.Math.Sqrt(errs[nextIndex]);
double R = a * b * c /
System.Math.Sqrt((a+b+c)*(-a+b+c)*(a-b+c) * (a+b-c));
hint = minIndex + offset;
double errOut = 0.5 * b * c / R;
if (double.IsNaN(errOut))
{
// Happens in round off for some triangles?
return errs[minIndex];
}
return errOut;
}
/// <summary>
/// Create the ideal path.
/// </summary>
protected void createPath()
{
// Strait for 100 counts in mostly Y.
double dt = 1 / (GPS_MeasurementHz * positionOverSampling);
Ideal_Time.Clear();
Ideal_X.Clear();
Ideal_Y.Clear();
double x = 0;
double y = 0;
double vx = 0;
double vy = 0;
double secondsToReAcquire = 20;
double velocity = 2; // 2 m/s.
double amount2ndIsInFront = AmountInFront; // meters.
double vt = amount2ndIsInFront / velocity; // 1.5 meter in front, at a velocity of 2 m/s = 0.75 seconds.
double initialAngle = 10;
double secondsToRunStrait = 100;
double accelSeconds = 4;
double x2s = amount2ndIsInFront * System.Math.Sin(initialAngle * System.Math.PI / 180);
double y2s = amount2ndIsInFront * System.Math.Cos(initialAngle * System.Math.PI / 180);
// Accelerate over accelSeconds to velocity.
double ax = System.Math.Sin(initialAngle * System.Math.PI / 180);
double ay = System.Math.Cos(initialAngle * System.Math.PI / 180);
List<double> xacc = new List<double>();
List<double> yacc = new List<double>();
double accelMax = 0.25; // 0.25 m/s^2.
// Hold still for secondsToReAcquire
for (double t = 0; t < secondsToReAcquire; t += dt)
{
xacc.Add(0);
yacc.Add(0);
}
// accel for accelSeconds.
for (double t = 0; t < accelSeconds; t += dt)
{
xacc.Add(accelMax * ax);
yacc.Add(accelMax * ay);
}
// Cruise for secondsToRunStrait.
for (double t = 0; t < secondsToRunStrait; t += dt)
{
xacc.Add(0);
yacc.Add(0);
}
// de-accel for accelSeconds.
for (double t = 0; t < accelSeconds; t += dt)
{
xacc.Add(-accelMax * ax);
yacc.Add(-accelMax * ay);
}
// Hold still for secondsToReAcquire
for (double t = 0; t < secondsToReAcquire; t += dt)
{
xacc.Add(0);
yacc.Add(0);
}
// accel, rotate.
double angular = 10;
double angle = initialAngle;
double time = 0;
int k = xacc.Count;
while (angle < initialAngle + 180)
{
angle += angular * dt;
double acc = time < accelSeconds ? accelMax * time / accelSeconds : accelMax;
xacc.Add(acc * System.Math.Sin(angle * System.Math.PI / 180));
yacc.Add(acc * System.Math.Cos(angle * System.Math.PI / 180));
vx += xacc[k] * dt;
vy += yacc[k] * dt;
x += vx * dt;
y += vy * dt;
time += dt;
k++;
}
// de-accel for accelSeconds.
initialAngle = angle;
for (double t = 0; t < accelSeconds; t += dt)
{
xacc.Add(-vx / accelSeconds);
yacc.Add(-vy / accelSeconds);
}
// Hold still for secondsToReAcquire
for (double t = 0; t < secondsToReAcquire; t += dt)
{
xacc.Add(0);
yacc.Add(0);
}
// accel for accelSeconds.
ax = System.Math.Sin(initialAngle * System.Math.PI / 180);
ay = System.Math.Cos(initialAngle * System.Math.PI / 180);
for (double t = 0; t < accelSeconds; t += dt)
{
xacc.Add(accelMax * ax);
yacc.Add(accelMax * ay);
}
// Cruise for secondsToRunStrait.
for (double t = 0; t < secondsToRunStrait; t += dt)
{
xacc.Add(0);
yacc.Add(0);
}
// de-accel for accelSeconds.
for (double t = 0; t < accelSeconds; t += dt)
{
xacc.Add(-accelMax * ax);
yacc.Add(-accelMax * ay);
}
// Hold still for secondsToReAcquire
for (double t = 0; t < secondsToReAcquire; t += dt)
{
xacc.Add(0);
yacc.Add(0);
}
time = 0;
vx = 0;
vy = 0;
x = 0;
y = 0;
for (int i = 0; i < xacc.Count; i++)
{
Ideal_Time.Add(time);
Ideal_X.Add(x);
Ideal_Y.Add(y);
vx += xacc[i] * dt;
vy += yacc[i] * dt;
x += vx * dt;
y += vy * dt;
time += dt;
}
}
public List<double> idealVX = new List<double>();
public List<double> idealVY = new List<double>();
/// <summary>
/// Simulate GPS.
/// </summary>
protected void simulateGPS()
{
RandomNumbers.Random r = new RandomNumbers.Random();
r.Seed = RandomSeed;
Meas_Time.Clear();
Meas_X.Clear();
Meas_Y.Clear();
Meas_VX.Clear();
Meas_VY.Clear();
List<double> idealX = new List<double>();
List<double> idealY = new List<double>();
idealVX.Clear();
idealVY.Clear();
for (int i = 0; i < Ideal_Time.Count; i += positionOverSampling)
{
Meas_Time.Add(Ideal_Time[i]);
idealX.Add(Ideal_X[i]);
idealY.Add(Ideal_Y[i]);
Meas_X.Add(Ideal_X[i] + r.NextGuass_Double(0, GPS_Position1Sigma));
Meas_Y.Add(Ideal_Y[i] + r.NextGuass_Double(0, GPS_Position1Sigma));
}
double dt = Meas_Time[1] - Meas_Time[0];
for (int i = 0; i < Meas_X.Count - 1; i++)
{
double vx = (idealX[i + 1] - idealX[i]) / dt;
double vy = (idealY[i + 1] - idealY[i]) / dt;
idealVX.Add(vx);
idealVY.Add(vy);
vx += r.NextGuass_Double(0, GPS_Velocity1Sigma);
vy += r.NextGuass_Double(0, GPS_Velocity1Sigma);
Meas_VX.Add(vx);
Meas_VY.Add(vy);
}
idealVX.Add(0);
idealVY.Add(0);
Meas_VX.Add(0);
Meas_VY.Add(0);
for (int i = 0; i < Meas_VX.Count - 1; i++)
{
double ax = (idealVX[i + 1] - idealVX[i]) / dt;
double ay = (idealVY[i + 1] - idealVY[i]) / dt;
ax += r.NextGuass_Double(0, Accelerometer_1Sigma);
ay += r.NextGuass_Double(0, Accelerometer_1Sigma);
Meas_AX.Add(ax);
Meas_AY.Add(ay);
}
Meas_AX.Add(0);
Meas_AY.Add(0);
}
}
}
|
By viewing downloads associated with this article you agree to the Terms of Service and the article's licence.
If a file you wish to view isn't highlighted, and is a text file (not binary), please
let us know and we'll add colourisation support for it.
Phil is a Principal Software developer focusing on weird yet practical algorithms that run the gamut of embedded and desktop (PID loops, Kalman filters, FFTs, client-server SOAP bindings, ASIC design, communication protocols, game engines, robotics).
In his personal life he is a part time mad scientist, full time dad, and studies small circle jujitsu, plays guitar and piano.
Submit an article or tip