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 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;
}
