public StochasticManifoldPoint Apply(IManifoldPoint point, double time)
{
var expectation = new Rotation3(time * _angularVelocity).Apply((Vector3)(OVector)point);
var covariance = SymmetricMatrix.Scalar(3, time * _diffusionCoefficient);
return(new StochasticManifoldPoint((OVector)expectation, covariance));
}
public StochasticManifoldPoint(IManifoldPoint expectation, OSymmetricMatrix covariance)
{
ArgAssert.Equal(expectation.Dimension, "expectation.Dimension", covariance.Dimension, "covariance.Dimension");
_expectation = expectation;
_covariance = covariance;
}
public State(IManifoldPoint point)
{
var productPoint = (ProductManifold.Point)point;
_basePoint = productPoint.Factors[(int)ManifoldProductOrder.Base];
_fiberPoint = productPoint.Factors[(int)ManifoldProductOrder.Fiber];
}
public static void ValidateProcess(IStochasticProcess process, IManifoldPoint point, double time, double eps, double tolerance)
{
var trivialEvolution = process.Apply(point, time: 0);
Assert.That(process.StateSpace.GetTranslation(trivialEvolution.Expectation, point).Norm(), Is.LessThan(_tolerance));
Assert.That(trivialEvolution.Covariance.FrobeniusNorm(), Is.LessThan(_tolerance));
ValidateMappingDifferential(process.Apply(time), point, eps, tolerance);
}
public StochasticManifoldPoint Apply(IManifoldPoint point, double time)
{
var relaxationFactor = Math.Exp(-time / RelaxationTime);
var expectation = relaxationFactor * (OVector)point + (1 - relaxationFactor) * AttractorPoint;
var covariance = (RelaxationTime * (1 - relaxationFactor * relaxationFactor) / 2) * DiffusionMatrix;
return(new StochasticManifoldPoint(expectation, covariance));
}
private IManifold MockManifold(int dimension, IManifoldPoint untranslatedPoint, OVector translationVector, IManifoldPoint translatedPoint)
{
var answer = new Mock <IManifold>();
answer.Setup(man => man.Dimension).Returns(dimension);
answer.Setup(man => man.Translate(untranslatedPoint, translationVector)).Returns(translatedPoint);
answer.Setup(man => man.GetTranslation(translatedPoint, untranslatedPoint)).Returns(translationVector);
return(answer.Object);
}
public IManifoldPoint Translate(IManifoldPoint point, OVector tangentVector)
{
var nativePoint = (Point)point;
var answerFactors = new List <IManifoldPoint>(_factors.Count);
for (int i = 0; i < _factors.Count; i++)
{
answerFactors.Add(_factors[i].Translate(nativePoint.Factors[i], tangentVector.Subvector(_indices[i], _factors[i].Dimension)));
}
return(new Point(answerFactors.AsReadOnlyList()));
}
public OVector GetTranslation(IManifoldPoint to, IManifoldPoint from)
{
var nativeTo = (Point)to;
var nativeFrom = (Point)from;
var answer = new Vector(Dimension);
for (int i = 0; i < _factors.Count; i++)
{
answer.SetSubvector(_indices[i], _factors[i].GetTranslation(nativeTo.Factors[i], nativeFrom.Factors[i]));
}
return(answer);
}
public OSquareMatrix ExpectationDifferential(IManifoldPoint point, double time)
{
var productPoint = (ProductManifold.Point)point;
var answer = new SquareMatrix(_stateSpace.Dimension);
for (int i = 0; i < _factors.Count; i++)
{
var index = _stateSpace.CoordinateIndex(i);
var factorDifferential = _factors[i].ExpectationDifferential(productPoint.Factors[i], time);
answer.SetSubmatrix(index, index, factorDifferential);
}
return(answer);
}
public StochasticManifoldPoint Apply(IManifoldPoint point, double time)
{
var productPoint = (ProductManifold.Point)point;
var answerFactors = new List <IManifoldPoint>(_factors.Count);
var answerCovariance = new SymmetricMatrix(_stateSpace.Dimension);
for (int i = 0; i < _factors.Count; i++)
{
var stochasticFactor = _factors[i].Apply(productPoint.Factors[i], time);
answerFactors.Add(stochasticFactor.Expectation);
answerCovariance.SetSubmatrix(_stateSpace.CoordinateIndex(i), stochasticFactor.Covariance);
}
return(new StochasticManifoldPoint(new ProductManifold.Point(answerFactors.AsReadOnlyList()), answerCovariance));
}
public StochasticManifoldPoint Apply(IManifoldPoint point, double time)
{
var state = new State(point);
var basePoint = _baseProcess.Apply(state.BasePoint, time);
var fiberPoint = FiberEvolution(state, basePoint, time);
var answerExpectation = new ProductManifold.Point(basePoint.Expectation, fiberPoint.Point.Expectation);
var answerCovariance = new SymmetricMatrix(_stateSpace.Dimension);
answerCovariance.SetSubmatrix(0, basePoint.Covariance);
answerCovariance.SetSubmatrix(_baseProcess.StateSpace.Dimension, fiberPoint.Point.Covariance);
answerCovariance.SetSubmatrix(_baseProcess.StateSpace.Dimension, 0, fiberPoint.MixedCovariance);
return(new StochasticManifoldPoint(answerExpectation, answerCovariance));
}
public OSquareMatrix ExpectationDifferential(IManifoldPoint point, double time)
{
var state = new State(point);
var basePoint = _baseProcess.Apply(state.BasePoint, time);
var baseDifferential = _baseProcess.ExpectationDifferential(state.BasePoint, time);
var mixedDifferential = MixedDifferential(state, basePoint.Expectation, baseDifferential, time);
var fiberDifferential = FiberDifferential(state, basePoint.Expectation, time);
var answer = new SquareMatrix(_stateSpace.Dimension);
answer.SetSubmatrix(0, 0, baseDifferential);
answer.SetSubmatrix(_baseProcess.StateSpace.Dimension, 0, mixedDifferential);
answer.SetSubmatrix(_baseProcess.StateSpace.Dimension, _baseProcess.StateSpace.Dimension, fiberDifferential);
return(answer);
}
// 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));
}
public OVector GetTranslation(IManifoldPoint to, IManifoldPoint from)
{
return(new Vector((((Rotation2)from).Inv * (Rotation2)to).Angle));
}
public IManifoldPoint Translate(IManifoldPoint point, OVector tangentVector)
{
ArgAssert.Equal(tangentVector.Dimension, "tangentVector.Dimension", 1, "1");
return((Rotation2)point * new Rotation2(tangentVector[0]));
}
protected override OMatrix VelocityDifferential(IManifoldPoint basePoint)
{
return(Matrix.Id(Fiber.Dimension));
}
protected override OVector Velocity(IManifoldPoint basePoint)
{
return((OVector)basePoint);
}
public StochasticManifoldPoint Apply(IManifoldPoint point)
{
return(new StochasticManifoldPoint(_matrix * (OVector)point + _freeTerm, _covariance));
}
public OVector GetTranslation(IManifoldPoint to, IManifoldPoint from)
{
return((OVector)to - (OVector)from);
}
protected abstract OVector Velocity(IManifoldPoint basePoint);
public OSquareMatrix ExpectationDifferential(IManifoldPoint point, double time)
{
return(SquareMatrix.Id(Dimension));
}
public OSquareMatrix ExpectationDifferential(IManifoldPoint point, double time)
{
return(new Rotation3(time * _angularVelocity).Matrix());
}
public OMatrix ExpectationDifferential(IManifoldPoint point)
{
return(_matrix);
}
public IManifoldPoint Translate(IManifoldPoint point, OVector tangentVector)
{
return((OVector)point + tangentVector);
}
protected abstract OMatrix VelocityDifferential(IManifoldPoint basePoint);
public static IManifoldPoint GetSubstate(IManifoldPoint state, int index)
{
return(((ProductManifold.Point)state).Factors[index]);
}
protected sealed override OSquareMatrix FiberDifferential(State state, IManifoldPoint basePoint2, double time)
{
return(SquareMatrix.Id(Fiber.Dimension));
}
public OSquareMatrix ExpectationDifferential(IManifoldPoint point, double time)
{
return(SquareMatrix.Scalar(StateSpace.Dimension, Math.Exp(-time / RelaxationTime)));
}
protected sealed override OMatrix MixedDifferential(State state, IManifoldPoint basePoint2, OSquareMatrix baseDifferential, double time)
{
return((time / 2) * (VelocityDifferential(state.BasePoint) + VelocityDifferential(basePoint2) * baseDifferential));
}
