public static StochasticManifoldPoint Apply(this IStochasticProcess process, StochasticManifoldPoint stochasticPoint, double time)
{
var diff = process.ExpectationDifferential(stochasticPoint.Expectation, time);
var prediction = process.Apply(stochasticPoint.Expectation, time);
return(new StochasticManifoldPoint(prediction.Expectation, stochasticPoint.Covariance.Conjugate(diff) + prediction.Covariance));
}
public KalmanFilter(IStochasticProcess processModel, StochasticManifoldPoint initialEstimate, double initialTime)
{
ArgAssert.NotNull(processModel, "processModel");
ArgAssert.Equal(processModel.StateSpace.Dimension, "processModel.StateSpace.Dimension", initialEstimate.Dimension, "initialEstimate.Dimension");
_time = initialTime;
_estimate = initialEstimate;
_processModel = processModel;
}
protected sealed override StochasticFiberPoint FiberEvolution(State state, StochasticManifoldPoint basePoint2, double time)
{
var expectation = Fiber.Translate(state.FiberPoint, (time / 2) * (Velocity(state.BasePoint) + Velocity(basePoint2.Expectation)));
var differential = (time / 2) * VelocityDifferential(basePoint2.Expectation);
var mixedCovariance = differential * basePoint2.Covariance;
var covariance = (mixedCovariance * differential.Transposed()).AsSquare().AsSymmetric();
return(new StochasticFiberPoint(new StochasticManifoldPoint(expectation, covariance), mixedCovariance));
}
// 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 void Predict(double time)
{
_estimate = _processModel.Apply(_estimate, time - _time);
_time = time;
}
public StochasticFiberPoint(StochasticManifoldPoint point, OMatrix mixedCovariance)
{
_point = point;
_mixedCovariance = mixedCovariance;
}
protected abstract StochasticFiberPoint FiberEvolution(State state, StochasticManifoldPoint basePoint2, double time);
