Matrix.Single transition; // State transition matrix (A).
#endregion Fields
#region Constructors
public Kalman(
Matrix.Single transition,
Matrix.Single measurement,
Matrix.Single measurementNoiseCovariance,
Matrix.Single processNoiseCovariance,
Matrix.Single initialState,
Matrix.Single initialErrorCovariance,
Matrix.Single control)
{
int dynamicParameters = transition.Order;
int measureParameters = measurementNoiseCovariance.Order;
this.transition = transition;
this.processNoiseCovariance = processNoiseCovariance;
this.measurement = measurement;
this.measurementNoiseCovariance = measurementNoiseCovariance;
this.statePostCorrected = initialState;
this.errorCovariancePosteriori = initialErrorCovariance;
this.statePredicted = new Kean.Math.Matrix.Single(1, dynamicParameters);
this.errorCovariancePriori = new Kean.Math.Matrix.Single(dynamicParameters);
this.gain = new Kean.Math.Matrix.Single(measureParameters, dynamicParameters);
this.control = control;
}
public Kalman(int dynamicParameters, int measureParameters, int controlParameters)
{
this.statePredicted = new Kean.Math.Matrix.Single(1, dynamicParameters);
this.statePostCorrected = new Kean.Math.Matrix.Single(1, dynamicParameters);
this.transition = Matrix.Single.Identity(dynamicParameters);
this.processNoiseCovariance = Matrix.Single.Identity(dynamicParameters);
this.measurement = new Kean.Math.Matrix.Single(dynamicParameters, measureParameters);
this.measurementNoiseCovariance = Matrix.Single.Identity(measureParameters);
this.errorCovariancePriori = new Kean.Math.Matrix.Single(dynamicParameters);
this.errorCovariancePosteriori = new Kean.Math.Matrix.Single(dynamicParameters);
this.gain = new Kean.Math.Matrix.Single(measureParameters, dynamicParameters);
if (controlParameters > 0)
this.control = new Kean.Math.Matrix.Single(controlParameters, dynamicParameters);
}
public Matrix.Single Correct(Matrix.Single measurement)
{
Matrix.Single result = this.statePostCorrected;
if (measurement.NotNull())
{
// a = H*P'(k)
Matrix.Single b = this.measurement * this.errorCovariancePriori;
// b = temp2*Ht + R
Matrix.Single a = b * this.measurement.Transpose() + this.measurementNoiseCovariance;
// c = inv(a) * b
Matrix.Single c = a.Solve(b);
// K(k) = xt
this.gain = c.Transpose();
// x(k) = x'(k) + K(k)*(z(k) - H*x'(k))
result = this.statePostCorrected = this.statePredicted + this.gain * (measurement - this.measurement * this.statePredicted);
// P(k) = P'(k) - K(k)*temp2
this.errorCovariancePosteriori = this.errorCovariancePriori - this.gain * b;
}
return result;
}
public Matrix.Single Predict(Matrix.Single control)
{
Matrix.Single result;
// Update state
// x'(k) = A*x(k)
this.statePredicted = this.transition * this.statePostCorrected;
// x'(k) = x'(k) + B*u(k)
if (this.control.NotNull() && control.NotNull())
this.statePredicted += this.control * control;
// update error covariance
// P'(k) = A*P(k)*At + Q
this.errorCovariancePriori = (this.transition * this.errorCovariancePosteriori) * this.transition.Transpose() + this.processNoiseCovariance;
result = this.statePredicted;
return result;
}
Element[] Natural(Tuple<float, float>[] measures)
{
// Compute natural piecewise cubic splines.
Element[] result = new Element[measures.Length - 1];
Matrix.Single left = new Matrix.Single(measures.Length, measures.Length);
Matrix.Single right = new Matrix.Single(1, measures.Length);
left[0, 0] = 2;
left[1, 0] = 1;
left[measures.Length - 1, measures.Length - 1] = 2;
left[measures.Length - 2, measures.Length - 1] = 1;
for (int y = 1; y < measures.Length - 1; y++)
{
int x = y - 1;
left[x, y] = 1;
left[x + 1, y] = 4;
left[x + 2, y] = 1;
}
right[0, 0] = 3 * (measures[1].Item2 - measures[0].Item2);
right[0, measures.Length - 1] = 3 * (measures[measures.Length - 1].Item2 - measures[measures.Length - 2].Item2);
for (int y = 1; y < measures.Length - 1; y++)
right[0, y] = 3 * (measures[y + 1].Item2 - measures[y - 1].Item2);
Matrix.Single solution = left.Solve(right);
for (int x = 0; x < measures.Length - 1; x++)
{
float a = measures[x].Item2;
float b = solution[0, x];
float c = 3 * (measures[x + 1].Item2 - measures[x].Item2) - 2 * solution[0, x] - solution[0, x + 1];
float d = 2 * (measures[x].Item2 - measures[x + 1].Item2) + solution[0, x] + solution[0, x + 1];
result[x] = new Element(a, b, c, d, measures[x].Item1, measures[x + 1].Item1);
}
return result;
}
