public void Update_amounts_to_matrix_weighted_average_when_measurement_covariance_commutes_with_estimate_covariance()
{
var procVector = new Vector(3.141, 2.718, 0.577);
var procCovar = SymmetricMatrix.Scalar(dimension: 3, value: 0.321);
var procModel = new WienerProcess(procCovar, procVector);
OMatrix measMatrix = new Matrix(new double[, ]
{
{ -1, 2, -3 },
{ 0.4, -0.5, 6 },
{ -0.7, 0.8, -0.9 }
});
var measVector = new Vector(3.141, -2.718, 0.577);
OSymmetricMatrix measCovar = new SymmetricMatrix(new double[][]
{
new double[] { 1.0 },
new double[] { 0.5, 0.8 },
new double[] { 0, 0.4, 0.9 }
});
var measModel = new AffineStochasticTransformation(measMatrix, measVector, measCovar);
OVector initEstimate = new Vector(0.314, 0.718, 2.577);
var initCovar = new SymmetricMatrix(new double[][]
{
new double[] { 7 },
new double[] { 0, 7 },
new double[] { 0, 0, 7 }
});
var time = 7.3;
var filter = new KalmanFilter(procModel, new StochasticManifoldPoint(initEstimate, initCovar), time);
var invMeasMatrix = measMatrix.AsSquare().Inv();
var nativeMeasCovar = measCovar.Conjugate(invMeasMatrix);
for (; time < 15; time += 0.8)
{
OVector measurement = new Vector(0.3 * time, 0.2 * (time + 1), 0.1 * (time - 1));
var nativeMeas = invMeasMatrix * (measurement - measVector);
var pred = procModel.Apply(filter.Estimate, time - filter.Time);
var expectedEstimate = (pred.Covariance + nativeMeasCovar).Inv() * (nativeMeasCovar * (OVector)pred.Expectation + pred.Covariance * nativeMeas);
var expectedCovariance = (pred.Covariance + nativeMeasCovar).Inv() * pred.Covariance * nativeMeasCovar;
filter.Update(measModel, measurement, time);
ExpectVectorsAreAlmostEqual((OVector)filter.Estimate.Expectation, (OVector)expectedEstimate);
ExpectMatricesAreAlmostEqual(filter.Estimate.Covariance, expectedCovariance);
}
}
public void When_measurements_coincide_with_expectation_estimates_coincide_with_prediction()
{
var time = new Time {
Value = 7
};
var procModel = MockProcessModel();
var measModel = MockMeasurementModel(time);
OVector initE = new Vector(3.141, 2.718, 0.577);
var initCovar = new SymmetricMatrix(new double[][]
{
new double[] { 1 },
new double[] { 2, 3 },
new double[] { 3, 4, 5 }
});
var predictor = new KalmanFilter(procModel, new StochasticManifoldPoint(initE, initCovar), time.Value);
var filter = new KalmanFilter(procModel, new StochasticManifoldPoint(initE, initCovar), time.Value);
for (; time.Value < 15; time.Value += 1.1)
{
predictor.Predict(time.Value);
filter.Update(measModel, measModel.Apply(predictor.Estimate.Expectation).Expectation, time.Value);
ExpectVectorsAreAlmostEqual((OVector)filter.Estimate.Expectation, (OVector)predictor.Estimate.Expectation);
}
}
public void Estimate_after_construction_coincides_with_argument_provided_to_constructor()
{
var procModel = StubProcessModel();
OVector initExpectation = new Vector(3.141, 2.718, 0.577);
var initCovar = new SymmetricMatrix(new double[][]
{
new double[] { 1 },
new double[] { 2, 3 },
new double[] { 3, 4, 5 }
});
var filter = new KalmanFilter(procModel, new StochasticManifoldPoint(initExpectation, initCovar), initialTime: 7);
ExpectVectorsAreAlmostEqual((OVector)filter.Estimate.Expectation, initExpectation);
ExpectMatricesAreAlmostEqual(filter.Estimate.Covariance, initCovar);
}