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