/// <summary>
/// Creates a new Discrete Time Kalman Filter with the given values for
/// the initial state and the covariance of that state.
/// </summary>
/// <param name="x0">The best estimate of the initial state of the estimate.</param>
/// <param name="P0">The covariance of the initial state estimate. If unsure
/// about initial state, set to a large value</param>
public DiscreteKalmanFilter(Matrix <double> x0, Matrix <double> P0)
{
KalmanFilter.CheckInitialParameters(x0, P0);
x = x0;
P = P0;
}
/// <summary>
/// Perform a discrete time prediction of the system state.
/// </summary>
/// <param name="F">State transition matrix.</param>
/// <exception cref="System.ArgumentException">Thrown when the given state
/// transition matrix does not have the same number of row/columns as there
/// are variables in the state vector.</exception>
public void Predict(Matrix <double> F)
{
KalmanFilter.CheckPredictParameters(F, this);
x = F * x;
P = F * P * F.Transpose();
}
/// <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)
{
KalmanFilter.CheckPredictParameters(F, Q, this);
// Predict the state
x = F * x;
P = (F * P * F.Transpose()) + Q;
}
/// <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);
}
/// <summary>
/// Performs a prediction of the state of the system after a given transition.
/// </summary>
/// <param name="F">State transition matrix.</param>
/// <exception cref="System.ArgumentException">Thrown when the given state
/// transition matrix does not have the same number of row/columns as there
/// are variables in the state vector.</exception>
public void Predict(Matrix <double> F)
{
KalmanFilter.CheckPredictParameters(F, this);
Matrix <double> tmpG = Matrix <double> .Build.Dense(x.RowCount, 1);
Matrix <double> tmpQ = Matrix <double> .Build.Dense(1, 1, 1.0d);
Predict(F, tmpG, tmpQ);
}
/// <summary>
/// Creates a square root filter with given initial state.
/// </summary>
/// <param name="x0">Initial state estimate.</param>
/// <param name="P0">Covariance of initial state estimate.</param>
public SquareRootFilter(Matrix <double> x0, Matrix <double> P0)
{
KalmanFilter.CheckInitialParameters(x0, P0);
// Decompose the covariance matrix
Matrix <double>[] UDU = UDUDecomposition(P0);
U = UDU[0];
D = UDU[1];
x = x0;
}
/// <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)
{
KalmanFilter.CheckPredictParameters(F, G, Q, this);
// State prediction
x = F * x;
// Covariance update
P = (F * P * F.Transpose()) + (G * Q * G.Transpose());
}
/// <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>
/// 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);
// We need to use transpose of H a couple of times.
Matrix <double> Ht = H.Transpose();
Matrix <double> I = Matrix <double> .Build.DenseIdentity(x.RowCount, x.RowCount);
Matrix <double> S = (H * P * Ht) + R; // Measurement covariance
Matrix <double> K = P * Ht * S.Inverse(); // Kalman Gain
P = (I - (K * H)) * P; // Covariance update
x = x + (K * (z - (H * x))); // State update
}
/// <summary>
/// Perform a discrete time prediction of the system state.
/// </summary>
/// <param name="F">State transition matrix.</param>
/// <exception cref="System.ArgumentException">Thrown when the given state
/// transition matrix does not have the same number of row/columns as there
/// are variables in the state vector.</exception>
public void Predict(Matrix <double> F)
{
KalmanFilter.CheckPredictParameters(F, this);
// Easier just to convert back to discrete form....
Matrix <double> p = J.Inverse();
Matrix <double> x = p * y;
x = F * x;
p = F * p * F.Transpose();
J = p.Inverse();
y = J * x;
}
/// <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>
/// Performs a prediction of the state of the system after a given transition.
/// </summary>
/// <param name="F">State transition matrix.</param>
/// <param name="G">Noise coupling matrix.</param>
/// <param name="Q">Noise covariance matrix.</param>
/// <exception cref="System.ArgumentException">Thrown when the given matrices
/// are not the correct dimensions.</exception>
public void Predict(Matrix <double> F, Matrix <double> G, Matrix <double> Q)
{
KalmanFilter.CheckPredictParameters(F, G, Q, this);
// Update the state.. that is easy!!
x = F * x;
// Get all the sized and create storage
int n = x.RowCount;
int p = G.ColumnCount;
Matrix <double> outD = Matrix <double> .Build.Dense(n, n); // Updated diagonal matrix
Matrix <double> outU = Matrix <double> .Build.DenseIdentity(n, n); // Updated upper unit triangular
// Get the UD Decomposition of the process noise matrix
Matrix <double>[] UDU = UDUDecomposition(Q);
Matrix <double> Uq = UDU[0];
Matrix <double> Dq = UDU[1];
// Combine it with the noise coupling matrix
Matrix <double> Gh = G * Uq;
// Convert state transition matrix
Matrix <double> PhiU = F * U;
// Ready to go..
for (int i = n - 1; i >= 0; i--)
{
// Update the i'th diagonal of the covariance matrix
double sigma = 0.0d;
for (int j = 0; j < n; j++)
{
sigma = sigma + (PhiU[i, j] * PhiU[i, j] * D[j, j]);
}
for (int j = 0; j < p; j++)
{
sigma = sigma + (Gh[i, j] * Gh[i, j] * Dq[j, j]);
}
outD[i, i] = sigma;
// Update the i'th row of the upper triangular of covariance
for (int j = 0; j < i; j++)
{
sigma = 0.0d;
for (int k = 0; k < n; k++)
{
sigma = sigma + (PhiU[i, k] * D[k, k] * PhiU[j, k]);
}
for (int k = 0; k < p; k++)
{
sigma = sigma + (Gh[i, k] * Dq[k, k] * Gh[j, k]);
}
outU[j, i] = sigma / outD[i, i];
for (int k = 0; k < n; k++)
{
PhiU[j, k] = PhiU[j, k] - (outU[j, i] * PhiU[i, k]);
}
for (int k = 0; k < p; k++)
{
Gh[j, k] = Gh[j, k] - (outU[j, i] * Gh[i, k]);
}
}
}
// Update the covariance
U = outU;
D = outD;
}
