Predict(
Matrix F
)
{
KalmanFilter.CheckPredictParameters(F, this);
this.x = F * this.x;
this.P = (F * P * Matrix.Transpose(F));
}
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;
}
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));
}
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;
}
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;
}