public void FitGaussianNoMap()
{
Pose3D linearpoint = new Pose3D();
double[] estimate = new double[6];
Map map = new Map(3);
var measurements = new List <PixelRangeMeasurement>();
double[][] covariance = MatrixExtensions.Zero(6).SingularInverse();
Gaussian expected = new Gaussian(new double[6] {
0, 0, 0, 0, 0, 0
}, covariance, 1.0);
Gaussian result = LoopyPHDNavigator.FitGaussian(estimate, measurements, map, linearpoint);
Assert.IsTrue(expected.Equals(result, 1e-5));
}