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