// predict + update
public void Update(IStochasticMapping measurementModel, IManifoldPoint measurement, double time)
{
ArgAssert.NotNull(measurementModel, "measurementModel");
ArgAssert.Equal(measurementModel.Domain.Dimension, "measurementModel.Domain.Dimension", _estimate.Dimension, "_estimate.Dimension");
ArgAssert.Equal(measurementModel.Codomain.Dimension, "measurementModel.Codomain.Dimension", measurement.Dimension, "measurement.Dimension");
Predict(time);
var measDiff = measurementModel.ExpectationDifferential(_estimate.Expectation);
var expectedMeasurement = measurementModel.Apply(_estimate.Expectation);
var gain = _estimate.Covariance * measDiff.Transposed() * (_estimate.Covariance.Conjugate(measDiff) + expectedMeasurement.Covariance).Inv();
var error = measurementModel.Codomain.GetTranslation(measurement, expectedMeasurement.Expectation);
var updatedExpectation = _processModel.StateSpace.Translate(_estimate.Expectation, gain * error);
var kh = (gain * measDiff).AsSquare();
var updatedCovariance = ((kh.Id() - kh) * _estimate.Covariance).AsSymmetric();
_estimate = new StochasticManifoldPoint(updatedExpectation, updatedCovariance);
}
public static void ValidateMappingDifferential(IStochasticMapping mapping, IManifoldPoint point, double eps, double tolerance)
{
var dim = mapping.Domain.Dimension;
var codim = mapping.Codomain.Dimension;
var expectedDifferential = new Matrix(codim, dim);
for (int i = 0; i < dim; i++)
{
var advancedPoint = mapping.Domain.Translate(point, +eps * Vector.Basis(i, dim));
var retardedPoint = mapping.Domain.Translate(point, -eps * Vector.Basis(i, dim));
var advancedImage = mapping.Apply(advancedPoint).Expectation;
var retardedImage = mapping.Apply(retardedPoint).Expectation;
var gradient = mapping.Codomain.GetTranslation(advancedImage, retardedImage) / (2 * eps);
expectedDifferential.SetColumn(i, gradient);
}
var actualDifferential = mapping.ExpectationDifferential(point);
Assert.That((expectedDifferential - actualDifferential).FrobeniusNorm(), Is.LessThan(tolerance));
}
