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,
});
}
private static Matrix ToInputMatrix(GPSFilter2DInput input)
{
return(Matrix.Create(new double[p, 1]
{
// {0}, // Px
// {0}, // Py
// {0}, // Pz
{ input.Velocity.X }, // Vx
{ input.Velocity.Y }, // Vy
{ input.Velocity.Z }, // Vz
{ input.Acceleration.X }, // Ax
{ input.Acceleration.Y }, // Ay
{ input.Acceleration.Z }, // Az
}));
}
private static Matrix ToInputMatrix(GPSFilter2DInput input)
{
return Matrix.Create(new double[p,1]
{
// {0}, // Px
// {0}, // Py
// {0}, // Pz
{input.Velocity.X}, // Vx
{input.Velocity.Y}, // Vy
{input.Velocity.Z}, // Vz
{input.Acceleration.X}, // Ax
{input.Acceleration.Y}, // Ay
{input.Acceleration.Z}, // Az
});
}
// 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);
}