public void TestSquareRootFilter()
{
System.Console.WriteLine("Filter 1 - DiscreteKalmanFilter, Filter 2 - SquareRootFilter");
Matrix x0 = RangeBearingTracker.TwoPointDifferenceState(rM[0], rM[1], bM[0], bM[1], T);
Matrix P0 = RangeBearingTracker.TwoPointDifferenceCovariance(rM[0], rM[1], bM[0], bM[1], re, the, T);
DiscreteKalmanFilter dkf = new DiscreteKalmanFilter(x0, P0);
SquareRootFilter sqrf = new SquareRootFilter(x0, P0);
Assert.IsTrue(RunTest(dkf, sqrf, DefaultTolerance));
}
public void TestSquareRootFilter()
{
System.Console.WriteLine("Filter 1 - DiscreteKalmanFilter, Filter 2 - SquareRootFilter");
Matrix<double> x0 = RangeBearingTracker.TwoPointDifferenceState(rM[0], rM[1], bM[0], bM[1], T);
Matrix<double> P0 = RangeBearingTracker.TwoPointDifferenceCovariance(rM[0], rM[1], bM[0], bM[1], re, the, T);
DiscreteKalmanFilter dkf = new DiscreteKalmanFilter(x0, P0);
SquareRootFilter sqrf = new SquareRootFilter(x0, P0);
Assert.IsTrue(RunTest(dkf, sqrf, DefaultTolerance));
}