// 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>
/// 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>
/// 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);
}
void Update(double z, Matrix<double> H, double R)
{
Matrix<double> a = U.Transpose()*H.Transpose();
Matrix<double> b = D*a;
double dz = z - (H*x)[0, 0];
double alpha = R;
double gamma = 1d/alpha;
for (int j = 0; j < x.RowCount; j++)
{
double beta = alpha;
alpha = alpha + (a[j, 0]*b[j, 0]);
double lambda = -a[j, 0]*gamma;
gamma = 1d/alpha;
D[j, j] = beta*gamma*D[j, j];
for (int i = 0; i < j; i++)
{
beta = U[i, j];
U[i, j] = beta + (b[i, 0]*lambda);
b[i, 0] = b[i, 0] + (b[j, 0]*beta);
}
}
double dzs = gamma*dz;
x = x + (dzs*b);
}
/// <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;
}
private void UpdateTimeVaryingMatrices(TimeSpan timeDelta)
{
double dt = timeDelta.TotalSeconds;
double posByV = dt; // p=vt
double posByA = 0.5 * dt * dt; // p=0.5at^2
double velByA = dt; // v=at
// World state transition matrix.
// Update position and velocity from previous state.
// Previous state acceleration is neglected since current acceleration only depends on current input.
A = Matrix.Create(new double[n, n]
{
// Px Py Pz Vx Vy Vz Qx Qy Qz Qw
{ 1, 0, 0, posByV, 0, 0, 0, 0, 0, 0 }, // Px
{ 0, 1, 0, 0, posByV, 0, 0, 0, 0, 0 }, // Py
{ 0, 0, 1, 0, 0, posByV, 0, 0, 0, 0 }, // Pz
{ 0, 0, 0, 1, 0, 0, 0, 0, 0, 0 }, // Vx
{ 0, 0, 0, 0, 1, 0, 0, 0, 0, 0 }, // Vy
{ 0, 0, 0, 0, 0, 1, 0, 0, 0, 0 }, // Vz
// We don't handle transition of quaternions here due to difficulties. Using B instead.
{ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 }, // Quaternion X
{ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 }, // Quaternion Y
{ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 }, // Quaternion Z
{ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 }, // Quaternion W
});
AT = Matrix.Transpose(A);
// Input gain matrix.
// Acceleration forward/right/down
// Angular Rate Roll/Pitch/Heading
B = Matrix.Create(new double[n, p]
{
// Ax Ay Az Qx Qy Qz Qw
{ posByA, 0, 0, 0, 0, 0, 0 }, // Px
{ 0, posByA, 0, 0, 0, 0, 0 }, // Py
{ 0, 0, posByA, 0, 0, 0, 0 }, // Pz
{ velByA, 0, 0, 0, 0, 0, 0 }, // Vx
{ 0, velByA, 0, 0, 0, 0, 0 }, // Vy
{ 0, 0, velByA, 0, 0, 0, 0 }, // Vz
// Simply set new orientation directly by quaternion input
{ 0, 0, 0, 1, 0, 0, 0 }, // Quaternion X
{ 0, 0, 0, 0, 1, 0, 0 }, // Quaternion Y
{ 0, 0, 0, 0, 0, 1, 0 }, // Quaternion Z
{ 0, 0, 0, 0, 0, 0, 1 }, // Quaternion W
});
// TODO For simplicity we assume all acceleromter axes have identical standard deviations (although they don't)
float accelStdDev = _sensorSpecifications.AccelerometerStdDev.Forward;
float velocityStdDev = ((float)velByA) * accelStdDev;
float positionStdDev = ((float)posByA) * accelStdDev;
// Diagonal matrix with noise std dev
Q = Matrix.Diagonal(new Vector(new double[n]
{
positionStdDev, positionStdDev, positionStdDev,
velocityStdDev, velocityStdDev, velocityStdDev,
0, 0, 0, 0 // TODO Orientation has no noise, should be added later
}));
R = Matrix.Diagonal(_stdDevV); // Diagonal matrix with noise std dev
Q *= Matrix.Transpose(Q); // Convert standard deviations to variances
R *= Matrix.Transpose(R); // Convert standard deviations to variances
w = GetNoiseMatrix(_stdDevW, n);
v = GetNoiseMatrix(_stdDevV, m);
}
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,
});
}
SingularValueDecomposition(IMatrix <double> arg)
{
_transpose = (arg.RowCount < arg.ColumnCount);
// Derived from LINPACK code.
// Initialize.
double[][] a;
if (_transpose)
{
// copy of internal data, independent of Arg
a = Matrix.Transpose(arg).GetArray();
_m = arg.ColumnCount;
_n = arg.RowCount;
}
else
{
a = arg.CopyToJaggedArray();
_m = arg.RowCount;
_n = arg.ColumnCount;
}
int nu = Math.Min(_m, _n);
double[] s = new double[Math.Min(_m + 1, _n)];
double[][] u = Matrix.CreateMatrixData(_m, nu);
double[][] v = Matrix.CreateMatrixData(_n, _n);
double[] e = new double[_n];
double[] work = new double[_m];
/*
* Reduce A to bidiagonal form, storing the diagonal elements
* in s and the super-diagonal elements in e.
*/
int nct = Math.Min(_m - 1, _n);
int nrt = Math.Max(0, Math.Min(_n - 2, _m));
for (int k = 0; k < Math.Max(nct, nrt); k++)
{
if (k < nct)
{
// Compute the transformation for the k-th column and
// place the k-th diagonal in s[k].
// Compute 2-norm of k-th column without under/overflow.
s[k] = 0;
for (int i = k; i < _m; i++)
{
s[k] = Fn.Hypot(s[k], a[i][k]);
}
if (s[k] != 0.0)
{
if (a[k][k] < 0.0)
{
s[k] = -s[k];
}
for (int i = k; i < _m; i++)
{
a[i][k] /= s[k];
}
a[k][k] += 1.0;
}
s[k] = -s[k];
}
for (int j = k + 1; j < _n; j++)
{
if ((k < nct) & (s[k] != 0.0))
{
/* Apply the transformation */
double t = 0;
for (int i = k; i < _m; i++)
{
t += a[i][k] * a[i][j];
}
t = (-t) / a[k][k];
for (int i = k; i < _m; i++)
{
a[i][j] += t * a[i][k];
}
}
/*
* Place the k-th row of A into e for the
* subsequent calculation of the row transformation.
*/
e[j] = a[k][j];
}
if (k < nct)
{
/*
* Place the transformation in U for subsequent back
* multiplication.
*/
for (int i = k; i < _m; i++)
{
u[i][k] = a[i][k];
}
}
if (k < nrt)
{
// Compute the k-th row transformation and place the
// k-th super-diagonal in e[k].
// Compute 2-norm without under/overflow.
e[k] = 0;
for (int i = k + 1; i < _n; i++)
{
e[k] = Fn.Hypot(e[k], e[i]);
}
if (e[k] != 0.0)
{
if (e[k + 1] < 0.0)
{
e[k] = -e[k];
}
for (int i = k + 1; i < _n; i++)
{
e[i] /= e[k];
}
e[k + 1] += 1.0;
}
e[k] = -e[k];
if ((k + 1 < _m) & (e[k] != 0.0))
{
/* Apply the transformation */
for (int i = k + 1; i < _m; i++)
{
work[i] = 0.0;
}
for (int j = k + 1; j < _n; j++)
{
for (int i = k + 1; i < _m; i++)
{
work[i] += e[j] * a[i][j];
}
}
for (int j = k + 1; j < _n; j++)
{
double t = (-e[j]) / e[k + 1];
for (int i = k + 1; i < _m; i++)
{
a[i][j] += t * work[i];
}
}
}
/*
* Place the transformation in V for subsequent
* back multiplication.
*/
for (int i = k + 1; i < _n; i++)
{
v[i][k] = e[i];
}
}
}
/* Set up the final bidiagonal matrix or order p. */
int p = Math.Min(_n, _m + 1);
if (nct < _n)
{
s[nct] = a[nct][nct];
}
if (_m < p)
{
s[p - 1] = 0.0;
}
if (nrt + 1 < p)
{
e[nrt] = a[nrt][p - 1];
}
e[p - 1] = 0.0;
/* If required, generate U */
for (int j = nct; j < nu; j++)
{
for (int i = 0; i < _m; i++)
{
u[i][j] = 0.0;
}
u[j][j] = 1.0;
}
for (int k = nct - 1; k >= 0; k--)
{
if (s[k] != 0.0)
{
for (int j = k + 1; j < nu; j++)
{
double t = 0;
for (int i = k; i < _m; i++)
{
t += u[i][k] * u[i][j];
}
t = (-t) / u[k][k];
for (int i = k; i < _m; i++)
{
u[i][j] += t * u[i][k];
}
}
for (int i = k; i < _m; i++)
{
u[i][k] = -u[i][k];
}
u[k][k] = 1.0 + u[k][k];
for (int i = 0; i < k - 1; i++)
{
u[i][k] = 0.0;
}
}
else
{
for (int i = 0; i < _m; i++)
{
u[i][k] = 0.0;
}
u[k][k] = 1.0;
}
}
/* If required, generate V */
for (int k = _n - 1; k >= 0; k--)
{
if ((k < nrt) & (e[k] != 0.0))
{
for (int j = k + 1; j < nu; j++)
{
double t = 0;
for (int i = k + 1; i < _n; i++)
{
t += v[i][k] * v[i][j];
}
t = (-t) / v[k + 1][k];
for (int i = k + 1; i < _n; i++)
{
v[i][j] += t * v[i][k];
}
}
}
for (int i = 0; i < _n; i++)
{
v[i][k] = 0.0;
}
v[k][k] = 1.0;
}
/* Main iteration loop for the singular values */
int pp = p - 1;
int iter = 0;
double eps = Number.PositiveRelativeAccuracy;
while (p > 0)
{
int k;
IterationStep step;
/* Here is where a test for too many iterations would go */
/*
* This section of the program inspects for
* negligible elements in the s and e arrays. On
* completion the variables kase and k are set as follows.
*
* DeflateNeglible: if s[p] and e[k-1] are negligible and k<p
* SplitAtNeglible: if s[k] is negligible and k<p
* QR: if e[k-1] is negligible, k<p, and s[k], ..., s[p] are not negligible.
* Convergence: if e[p-1] is negligible.
*/
for (k = p - 2; k >= 0; k--)
{
if (Math.Abs(e[k]) <= eps * (Math.Abs(s[k]) + Math.Abs(s[k + 1])))
{
e[k] = 0.0;
break;
}
}
if (k == p - 2)
{
step = IterationStep.Convergence;
}
else
{
int ks;
for (ks = p - 1; ks >= k; ks--)
{
if (ks == k)
{
break;
}
double t = (ks != p ? Math.Abs(e[ks]) : 0.0) + (ks != k + 1 ? Math.Abs(e[ks - 1]) : 0.0);
if (Math.Abs(s[ks]) <= eps * t)
{
s[ks] = 0.0;
break;
}
}
if (ks == k)
{
step = IterationStep.QR;
}
else if (ks == p - 1)
{
step = IterationStep.DeflateNeglible;
}
else
{
step = IterationStep.SplitAtNeglible;
k = ks;
}
}
k++;
/* Perform the task indicated by 'step'. */
switch (step)
{
// Deflate negligible s(p).
case IterationStep.DeflateNeglible:
{
double f = e[p - 2];
e[p - 2] = 0.0;
for (int j = p - 2; j >= k; j--)
{
double t = Fn.Hypot(s[j], f);
double cs = s[j] / t;
double sn = f / t;
s[j] = t;
if (j != k)
{
f = (-sn) * e[j - 1];
e[j - 1] = cs * e[j - 1];
}
for (int i = 0; i < _n; i++)
{
t = (cs * v[i][j]) + (sn * v[i][p - 1]);
v[i][p - 1] = ((-sn) * v[i][j]) + (cs * v[i][p - 1]);
v[i][j] = t;
}
}
}
break;
// Split at negligible s(k)
case IterationStep.SplitAtNeglible:
{
double f = e[k - 1];
e[k - 1] = 0.0;
for (int j = k; j < p; j++)
{
double t = Fn.Hypot(s[j], f);
double cs = s[j] / t;
double sn = f / t;
s[j] = t;
f = (-sn) * e[j];
e[j] = cs * e[j];
for (int i = 0; i < _m; i++)
{
t = (cs * u[i][j]) + (sn * u[i][k - 1]);
u[i][k - 1] = ((-sn) * u[i][j]) + (cs * u[i][k - 1]);
u[i][j] = t;
}
}
}
break;
// Perform one qr step.
case IterationStep.QR:
{
/* Calculate the shift */
double scale = Math.Max(Math.Max(Math.Max(Math.Max(Math.Abs(s[p - 1]), Math.Abs(s[p - 2])), Math.Abs(e[p - 2])), Math.Abs(s[k])), Math.Abs(e[k]));
double sp = s[p - 1] / scale;
double spm1 = s[p - 2] / scale;
double epm1 = e[p - 2] / scale;
double sk = s[k] / scale;
double ek = e[k] / scale;
double b = (((spm1 + sp) * (spm1 - sp)) + (epm1 * epm1)) / 2.0;
double c = (sp * epm1) * (sp * epm1);
double shift = 0.0;
if ((b != 0.0) | (c != 0.0))
{
shift = Math.Sqrt((b * b) + c);
if (b < 0.0)
{
shift = -shift;
}
shift = c / (b + shift);
}
double f = ((sk + sp) * (sk - sp)) + shift;
double g = sk * ek;
/* Chase zeros */
for (int j = k; j < p - 1; j++)
{
double t = Fn.Hypot(f, g);
double cs = f / t;
double sn = g / t;
if (j != k)
{
e[j - 1] = t;
}
f = (cs * s[j]) + (sn * e[j]);
e[j] = (cs * e[j]) - (sn * s[j]);
g = sn * s[j + 1];
s[j + 1] = cs * s[j + 1];
for (int i = 0; i < _n; i++)
{
t = (cs * v[i][j]) + (sn * v[i][j + 1]);
v[i][j + 1] = ((-sn) * v[i][j]) + (cs * v[i][j + 1]);
v[i][j] = t;
}
t = Fn.Hypot(f, g);
cs = f / t;
sn = g / t;
s[j] = t;
f = (cs * e[j]) + (sn * s[j + 1]);
s[j + 1] = ((-sn) * e[j]) + (cs * s[j + 1]);
g = sn * e[j + 1];
e[j + 1] = cs * e[j + 1];
if (j < _m - 1)
{
for (int i = 0; i < _m; i++)
{
t = (cs * u[i][j]) + (sn * u[i][j + 1]);
u[i][j + 1] = ((-sn) * u[i][j]) + (cs * u[i][j + 1]);
u[i][j] = t;
}
}
}
e[p - 2] = f;
iter = iter + 1;
}
break;
// Convergence.
case IterationStep.Convergence:
{
/* Make the singular values positive */
if (s[k] <= 0.0)
{
s[k] = (s[k] < 0.0 ? -s[k] : 0.0);
for (int i = 0; i <= pp; i++)
{
v[i][k] = -v[i][k];
}
}
/* Order the singular values */
while (k < pp)
{
if (s[k] >= s[k + 1])
{
break;
}
double t = s[k];
s[k] = s[k + 1];
s[k + 1] = t;
if (k < _n - 1)
{
for (int i = 0; i < _n; i++)
{
t = v[i][k + 1];
v[i][k + 1] = v[i][k];
v[i][k] = t;
}
}
if (k < _m - 1)
{
for (int i = 0; i < _m; i++)
{
t = u[i][k + 1];
u[i][k + 1] = u[i][k];
u[i][k] = t;
}
}
k++;
}
iter = 0;
p--;
}
break;
}
}
// (vermorel) transposing the results if needed
if (_transpose)
{
// swaping U and V
double[][] temp = v;
v = u;
u = temp;
}
_u = new Matrix(u);
_v = new Matrix(v);
_singular = new Vector(s);
InitOnDemandComputations();
}
/// <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>
/// <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>
/// 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>
/// 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();
}
