/// <summary>
/// Creates an Information filter with the given initial state.
/// </summary>
/// <param name="x0">Initial estimate of state variables.</param>
/// <param name="P0">Covariance of state variable estimates.</param>
public InformationFilter(Matrix<double> x0, Matrix<double> P0)
{
KalmanFilter.CheckInitialParameters(x0, P0);
J = P0.Inverse();
y = J*x0;
I = Matrix<double>.Build.DenseIdentity(y.RowCount, y.RowCount);
}
// 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>
/// Creates an Information filter specifying whether the covariance and state
/// have been 'inverted'.
/// </summary>
/// <param name="state">The initial estimate of the state of the system.</param>
/// <param name="cov">The covariance of the initial state estimate.</param>
/// <param name="inverted">Has covariance/state been converted to information
/// filter form?</param>
/// <remarks>This behaves the same as other constructors if the given boolean is false.
/// Otherwise, in relation to the given state/covariance should satisfy:<BR></BR>
/// <C>cov = J = P0 ^ -1, state = y = J * x0.</C></remarks>
public InformationFilter(Matrix<double> state, Matrix<double> cov, bool inverted)
{
KalmanFilter.CheckInitialParameters(state, cov);
if (inverted)
{
J = cov;
y = state;
}
else
{
J = cov.Inverse();
y = J*state;
}
I = Matrix<double>.Build.DenseIdentity(state.RowCount, state.RowCount);
}
/// <summary>
/// Updates the state of the system based on the given noisy measurements,
/// a description of how those measurements relate to the system, and a
/// covariance <c>Matrix</c> to describe the noise of the system.
/// </summary>
/// <param name="z">The measurements of the system.</param>
/// <param name="H">Measurement model.</param>
/// <param name="R">Covariance of measurements.</param>
/// <exception cref="System.ArgumentException">Thrown when given matrices
/// are of the incorrect size.</exception>
public void Update(Matrix<double> z, Matrix<double> H, Matrix<double> R)
{
KalmanFilter.CheckUpdateParameters(z, H, R, this);
// Fiddle with the matrices
Matrix<double> HT = H.Transpose();
Matrix<double> RI = R.Inverse();
// Perform the update
y = y + (HT*RI*z);
J = J + (HT*RI*H);
}
/// <summary>
/// Perform a discrete time prediction of the system state.
/// </summary>
/// <param name="F">State transition matrix.</param>
/// <param name="G">Noise coupling matrix.</param>
/// <param name="Q">Plant noise covariance.</param>
/// <exception cref="System.ArgumentException">Thrown when the column and row
/// counts for the given matrices are incorrect.</exception>
/// <remarks>
/// Performs a prediction of the next state of the Kalman Filter, given
/// a description of the dynamic equations of the system, the covariance of
/// the plant noise affecting the system and the equations that describe
/// the effect on the system of that plant noise.
/// </remarks>
public void Predict(Matrix<double> F, Matrix<double> G, Matrix<double> Q)
{
// Some matrices we will need a bit
Matrix<double> FI = F.Inverse();
Matrix<double> FIT = FI.Transpose();
Matrix<double> GT = G.Transpose();
Matrix<double> A = FIT*J*FI;
Matrix<double> B = A*G*(GT*A*G + Q.Inverse()).Inverse();
J = (I - B*GT)*A;
y = (I - B*GT)*FIT*y;
}
/// <summary>
/// Preform a discrete time prediction of the system state.
/// </summary>
/// <param name="F">State transition matrix.</param>
/// <param name="Q">A plant noise covariance matrix.</param>
/// <exception cref="System.ArgumentException">Thrown when F and Q are not
/// square matrices with the same number of rows and columns as there are
/// rows in the state matrix.</exception>
/// <remarks>Performs a prediction of the next state of the Kalman Filter,
/// where there is plant noise. The covariance matrix of the plant noise, in
/// this case, is a square matrix corresponding to the state transition and
/// the state of the system.</remarks>
public void Predict(Matrix<double> F, Matrix<double> Q)
{
// We will need these matrices more than once...
Matrix<double> FI = F.Inverse();
Matrix<double> FIT = FI.Transpose();
Matrix<double> A = FIT*J*FI;
Matrix<double> AQI = (A + Q.Inverse()).Inverse();
// 'Covariance' Update
J = A - (A*AQI*A);
y = (I - (A*AQI))*FIT*y;
}
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,
});
}
