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>
/// Transform sensor measurements to output matrix
/// </summary>
/// <param name="s"></param>
/// <returns></returns>
private static Matrix ToOutputMatrix(GPSObservation s)
{
return(Matrix.Create(new double[m, 1]
{
{ s.Position.X },
{ s.Position.Y },
{ s.Position.Z },
// {0}, {0}, {0}, // Velocity
// {0}, {0}, {0}, // Acceleration
// {s.Time.TotalSeconds},
}));
}
/// <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 GPSObservation
{
Position = GetPosition(x0),
Time = TimeSpan.Zero,
//GetTime(x0)
};
return(CalculateNext(startingCondition, observation, new GPSFilter2DInput()));
}
/// <summary></summary>
public void GPSFilter2DTest()
{
try
{
if (Chart == null)
return;
// Add test Lines to demonstrate the control
Chart.Primitives.Clear();
var filterStartState = new GPSObservation
{
Position = new Vector3(0, 0, 0),
// Velocity = new Vector3(1, 0, 0),
// Acceleration = new Vector3(0, 1, 0),
Time = TimeSpan.Zero,
};
var filter = new GPSFilter2D(filterStartState);
var inValues = new List<GPSObservation>();
for (int t = 0; t < 20; t++)
{
var obs = new GPSObservation
{
Position = new Vector3(t, 2.5f*0.01f*t*t, 0),
//(float) (1 + 1*Math.Cos(2*Math.PI*4*t/100)), 0),
Time = TimeSpan.FromSeconds(t)
};
inValues.Add(obs);
}
var outValues = new List<StepOutput<GPSFilter2DSample>>();
foreach (GPSObservation observation in inValues)
outValues.Add(filter.Filter(observation, new GPSFilter2DInput
{
Velocity = new Vector3(1, 0, 0),
Acceleration = new Vector3(0, 0.01f, 0),
}));
// IEnumerable<StepOutput<GPSFilter2DSample>> outValues = filter.Filter(inValues);
// AddLineXY("N-Window Smoothing", Colors.DarkGoldenrod, originalZValues);//smoothedZValues);
// AddLineXY("Noise W", Colors.White, outValues.Select(element => element.W));
// AddLineXY("Noise V", Colors.LightGray, outValues.Select(e => e.V.Position));
// AddLineXY("k", Colors.Cyan, outValues.Select(element => element.K));
SwordfishGraphHelper.AddLineXY(Chart, "A Priori X", Colors.Blue, outValues.Select(e => e.PriX.Position));
SwordfishGraphHelper.AddLineXY(Chart, "A Posteriori X", Colors.YellowGreen, outValues.Select(e => e.PostX.Position));
SwordfishGraphHelper.AddLineXY(Chart, "X", Color.FromArgb(255, 150, 0, 0), outValues.Select(e => e.X.Position));
SwordfishGraphHelper.AddLineXY(Chart, "Z", Color.FromArgb(255, 255, 255, 0), outValues.Select(e => e.Z.Position));
// AddLineXY(Chart, "True", Color.FromArgb(50, 150, 0, 0), inValues.Select(e => e.Position));
Chart.Title = "Filter2DTest()";
Chart.XAxisLabel = "X [m]";
Chart.YAxisLabel = "Y [m]";
Chart.RedrawPlotLines();
}
catch (Exception e)
{
MessageBox.Show(e.ToString());
}
}
public GPSFilter2D(GPSObservation startState)
{
X0 = Matrix.Create(new double[n, 1]
{
{ startState.Position.X }, { startState.Position.Y }, { startState.Position.Z },
// Position
{ 0 }, { 0 }, { 0 }, // Velocity
{ 0 }, { 0 }, { 0 }, // Acceleration
});
PostX0 = X0.Clone();
/* Matrix.Create(new double[n, 1]
* {
* {startState.Position.X}, {startState.Position.Y}, {startState.Position.Z}, // Position
* {1}, {0}, {0}, // Velocity
* {0}, {1}, {0}, // Acceleration
* });*/
// Start by assuming no covariance between states, meaning position, velocity, acceleration and their three XYZ components
// have no correlation and behave independently. This not entirely true.
PostP0 = Matrix.Identity(n, n);
// Refs:
// http://www.romdas.com/technical/gps/gps-acc.htm
// http://www.sparkfun.com/datasheets/GPS/FV-M8_Spec.pdf
// http://onlinestatbook.com/java/normalshade.html
// Assuming GPS Sensor: FV-M8
// Cold start: 41s
// Hot start: 1s
// Position precision: 3.3m CEP (horizontal circle, half the points within this radius centred on truth)
// Position precision (DGPS): 2.6m CEP
// const float coVarQ = ;
// _r = covarianceR;
// Determine standard deviation of estimated process and observation noise variance
// Position process noise
_stdDevW = Vector.Zeros(n); //(float)Math.Sqrt(_q);
_rand = new GaussianRandom();
// Circle Error Probable (50% of the values are within this radius)
// const float cep = 3.3f;
// GPS position observation noise by standard deviation [meters]
// Assume gaussian distribution, 2.45 x CEP is approx. 2dRMS (95%)
// ref: http://www.gmat.unsw.edu.au/snap/gps/gps_survey/chap2/243.htm
// Found empirically by http://onlinestatbook.com/java/normalshade.html
// using area=0.5 and limits +- 3.3 meters
_stdDevV = new Vector(new double[m]
{
0,
ObservationNoiseStdDevY,
0,
// 0,
// 0,
// 0,
// 0,
// 0,
// 0,
// 0,
});
//Vector.Zeros(Observations);
//2, 2, 4.8926f);
H = Matrix.Identity(m, n);
I = Matrix.Identity(n, n);
Q = new Matrix(n, n);
R = Matrix.Identity(m, m);
_prevEstimate = GetInitialEstimate(X0, PostX0, PostP0);
}
// 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,
};
}
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);
}
/// <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 GPSObservation
{
Position = GetPosition(x0),
Time = TimeSpan.Zero,
//GetTime(x0)
};
return CalculateNext(startingCondition, observation, new GPSFilter2DInput());
}
/// <summary>
/// Transform sensor measurements to output matrix
/// </summary>
/// <param name="s"></param>
/// <returns></returns>
private static Matrix ToOutputMatrix(GPSObservation s)
{
return Matrix.Create(new double[m,1]
{
{s.Position.X},
{s.Position.Y},
{s.Position.Z},
// {0}, {0}, {0}, // Velocity
// {0}, {0}, {0}, // Acceleration
// {s.Time.TotalSeconds},
});
}
public GPSFilter2D(GPSObservation startState)
{
X0 = Matrix.Create(new double[n,1]
{
{startState.Position.X}, {startState.Position.Y}, {startState.Position.Z},
// Position
{0}, {0}, {0}, // Velocity
{0}, {0}, {0}, // Acceleration
});
PostX0 = X0.Clone();
/* Matrix.Create(new double[n, 1]
{
{startState.Position.X}, {startState.Position.Y}, {startState.Position.Z}, // Position
{1}, {0}, {0}, // Velocity
{0}, {1}, {0}, // Acceleration
});*/
// Start by assuming no covariance between states, meaning position, velocity, acceleration and their three XYZ components
// have no correlation and behave independently. This not entirely true.
PostP0 = Matrix.Identity(n, n);
// Refs:
// http://www.romdas.com/technical/gps/gps-acc.htm
// http://www.sparkfun.com/datasheets/GPS/FV-M8_Spec.pdf
// http://onlinestatbook.com/java/normalshade.html
// Assuming GPS Sensor: FV-M8
// Cold start: 41s
// Hot start: 1s
// Position precision: 3.3m CEP (horizontal circle, half the points within this radius centred on truth)
// Position precision (DGPS): 2.6m CEP
// const float coVarQ = ;
// _r = covarianceR;
// Determine standard deviation of estimated process and observation noise variance
// Position process noise
_stdDevW = Vector.Zeros(n); //(float)Math.Sqrt(_q);
_rand = new GaussianRandom();
// Circle Error Probable (50% of the values are within this radius)
// const float cep = 3.3f;
// GPS position observation noise by standard deviation [meters]
// Assume gaussian distribution, 2.45 x CEP is approx. 2dRMS (95%)
// ref: http://www.gmat.unsw.edu.au/snap/gps/gps_survey/chap2/243.htm
// Found empirically by http://onlinestatbook.com/java/normalshade.html
// using area=0.5 and limits +- 3.3 meters
_stdDevV = new Vector(new double[m]
{
0,
ObservationNoiseStdDevY,
0,
// 0,
// 0,
// 0,
// 0,
// 0,
// 0,
// 0,
});
//Vector.Zeros(Observations);
//2, 2, 4.8926f);
H = Matrix.Identity(m, n);
I = Matrix.Identity(n, n);
Q = new Matrix(n, n);
R = Matrix.Identity(m, m);
_prevEstimate = GetInitialEstimate(X0, PostX0, PostP0);
}