Predict(
Matrix F
)
{
KalmanFilter.CheckPredictParameters(F, this);
this.x = F * this.x;
this.P = (F * P * Matrix.Transpose(F));
}
DiscreteKalmanFilter(
Matrix x0,
Matrix P0
)
{
KalmanFilter.CheckInitialParameters(x0, P0);
this.x = x0;
this.P = P0;
}
Predict(
Matrix F,
Matrix Q
)
{
KalmanFilter.CheckPredictParameters(F, Q, this);
// Predict the state
this.x = F * x;
this.P = (F * P * Matrix.Transpose(F)) + Q;
}
InformationFilter(
Matrix x0,
Matrix P0
)
{
KalmanFilter.CheckInitialParameters(x0, P0);
this.J = P0.Inverse();
this.y = this.J * x0;
this.I = Matrix.Identity(this.y.RowCount, this.y.RowCount);
}
Predict(
Matrix F
)
{
KalmanFilter.CheckPredictParameters(F, this);
Matrix tmpG = new Matrix(this.x.RowCount, 1, 0.0d);
Matrix tmpQ = new Matrix(1, 1, 1.0d);
this.Predict(F, tmpG, tmpQ);
}
Predict(
Matrix F,
Matrix G,
Matrix Q
)
{
KalmanFilter.CheckPredictParameters(F, G, Q, this);
// State prediction
this.x = F * x;
// Covariance update
this.P = (F * P * Matrix.Transpose(F)) + (G * Q * Matrix.Transpose(G));
}
SquareRootFilter(
Matrix x0,
Matrix P0
)
{
KalmanFilter.CheckInitialParameters(x0, P0);
// Decompose the covariance matrix
Matrix[] UDU = UDUDecomposition(P0);
this.U = UDU[0];
this.D = UDU[1];
this.x = x0;
}
Update(
Matrix z,
Matrix H,
Matrix R
)
{
KalmanFilter.CheckUpdateParameters(z, H, R, this);
// Fiddle with the matrices
Matrix HT = Matrix.Transpose(H);
Matrix RI = R.Inverse();
// Perform the update
this.y = this.y + (HT * RI * z);
this.J = this.J + (HT * RI * H);
}
Predict(
Matrix F
)
{
KalmanFilter.CheckPredictParameters(F, this);
// Easier just to convert back to discrete form....
Matrix P = this.J.Inverse();
Matrix x = P * this.y;
x = F * x;
P = F * P * Matrix.Transpose(F);
this.J = P.Inverse();
this.y = this.J * x;
}
Update(
Matrix z,
Matrix H,
Matrix R
)
{
KalmanFilter.CheckUpdateParameters(z, H, R, this);
// We need to use transpose of H a couple of times.
Matrix Ht = Matrix.Transpose(H);
Matrix I = Matrix.Identity(x.RowCount, x.RowCount);
Matrix S = (H * P * Ht) + R; // Measurement covariance
Matrix K = P * Ht * S.Inverse(); // Kalman Gain
this.P = (I - (K * H)) * P; // Covariance update
this.x = this.x + (K * (z - (H * this.x))); // State update
}
InformationFilter(
Matrix state,
Matrix cov,
bool inverted
)
{
KalmanFilter.CheckInitialParameters(state, cov);
if (inverted)
{
this.J = cov;
this.y = state;
}
else
{
this.J = cov.Inverse();
this.y = this.J * state;
}
this.I = Matrix.Identity(state.RowCount, state.RowCount);
}
Predict(
Matrix F,
Matrix G,
Matrix Q
)
{
KalmanFilter.CheckPredictParameters(F, G, Q, this);
// Update the state.. that is easy!!
this.x = F * this.x;
// Get all the sized and create storage
int n = this.x.RowCount;
int p = G.ColumnCount;
Matrix outD = new Matrix(n, n); // Updated diagonal matrix
Matrix outU = Matrix.Identity(n, n); // Updated upper unit triangular
// Get the UD Decomposition of the process noise matrix
Matrix[] UDU = UDUDecomposition(Q);
Matrix Uq = UDU[0];
Matrix Dq = UDU[1];
// Combine it with the noise coupling matrix
Matrix Gh = G * Uq;
// Convert state transition matrix
Matrix PhiU = F * (new Matrix(this.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] * this.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] * this.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
this.U = outU;
this.D = outD;
}
