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 Apply_Covariance_results_from_base_covariance_by_conjugating_with_numeric_differential()
{
const double eps = 1e-5;
const double tolerance = 1e-7;
var diffusionMatrix = new SymmetricMatrix(new double[][]
{
new double [] { +0.3, },
new double [] { -0.1, +0.4, },
new double [] { +0.1, +0.2, +0.4 }
});
var baseProcess = new WienerProcess(diffusionMatrix);
var integralProcess = CreateIntegralProcess(baseProcess);
var evolution = integralProcess.Apply(_point, _time);
var baseResult = (OVector)ExtensionStochasticProcess.GetBaseState(evolution.Expectation);
var baseDim = baseProcess.StateSpace.Dimension;
var differential = new Matrix(integralProcess.StateSpace.Dimension, baseDim);
differential.SetSubmatrix(integralProcess.BaseCoordinateIndex(), 0, Matrix.Id(baseDim));
for (int j = 0; j < baseDim; j++)
{
var delta = eps * Vector.Basis(j, baseDim);
var retarted = baseResult - delta;
var advanced = baseResult + delta;
var gradient = (ExpectedFiberResult(advanced) - ExpectedFiberResult(retarted)) / (2 * eps);
differential.SetSubmatrix(integralProcess.FiberCoordinateIndex(), j, gradient.AsColumn());
}
var expectedCovariance = baseProcess.Apply(_basePoint, _time).Covariance.Conjugate(differential);
Expect((evolution.Covariance - expectedCovariance).FrobeniusNorm(), Is.LessThan(tolerance));
}
