private StepOutput <Matrix> CalculateNext(StepInput <Matrix> prev, GPSINSObservation observation,
GPSINSInput input)
{
TimeSpan time = observation.Time;
TimeSpan timeDelta = time - _prevEstimate.Time;
// Note: Use 0.0 and not 0 in order to use the correct constructor!
// If no GPS update is available, then use a zero matrix to ignore the values in the observation.
H = observation.GotGPSUpdate
? Matrix.Identity(m, n)
: new Matrix(m, n, 0.0);
HT = Matrix.Transpose(H);
UpdateTimeVaryingMatrices(timeDelta);
Matrix u = ToInputMatrix(input, time);
// 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; noise is already modelled in input data
Matrix z = ToObservationMatrix(observation); //H * x + v;// m by 1
// Predictor equations
Matrix PriX = A * prev.PostX + B * u; // n by 1
Matrix PriP = A * prev.PostP * AT + Q; // n by n
Matrix residual = z - H * PriX; // m by 1
Matrix residualP = H * PriP * HT + R; // m by m
// If residualP is zero matrix then set its inverse to zero as well.
// This occurs if all observation standard deviations are zero
// and this workaround will cause the Kalman gain to trust the process model entirely.
Matrix residualPInv = Matrix.AlmostEqual(residualP, _zeroMM) // m by m
? _zeroMM
: residualP.Inverse();
// Corrector equations
Matrix K = PriP * Matrix.Transpose(H) * residualPInv; // n by m
Matrix PostX = PriX + K * residual; // n by 1
Matrix PostP = (I - K * H) * PriP; // n by n
var tmpPosition = new Vector3((float)PostX[0, 0], (float)PostX[1, 0], (float)PostX[2, 0]);
// var tmpPrevPosition = new Vector3((float)prev.PostX[0, 0], (float)prev.PostX[1, 0], (float)prev.PostX[2, 0]);
// Vector3 positionChange = tmpPosition - tmpPrevPosition;
Vector3 gpsPositionTrust = new Vector3((float)K[0, 0], (float)K[1, 1], (float)K[2, 2]);
Vector3 gpsVelocityTrust = new Vector3((float)K[3, 3], (float)K[4, 4], (float)K[5, 5]);
//
// Matrix tmpPriPGain = A*Matrix.Identity(10,10)*AT + Q;
var tmpVelocity = new Vector3((float)x[3, 0], (float)x[4, 0], (float)x[5, 0]);
var tmpAccel = new Vector3((float)x[6, 0], (float)x[7, 0], (float)x[8, 0]);
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,
});
}