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));
}
private bool RunTest(IKalmanFilter filter1, IKalmanFilter filter2, double tolerance)
{
List <double> xf1 = new List <double>();
List <double> yf1 = new List <double>();
List <double> xf2 = new List <double>();
List <double> yf2 = new List <double>();
Matrix <double> ZeroCov = Matrix <double> .Build.Dense(filter1.Cov.RowCount, filter1.Cov.RowCount);
Matrix <double> ZeroState = Matrix <double> .Build.Dense(filter1.State.RowCount, 1);
RangeBearingTracker rbt1 = new RangeBearingTracker(filter1);
RangeBearingTracker rbt2 = new RangeBearingTracker(filter2);
bool withinTolerance = true;
// Predict the filters
rbt1.Predict(this.T, this.q);
rbt2.Predict(this.T, this.q);
for (int i = 2; i < this.bM.Length; i++)
{
rbt1.Update(this.rM[i], this.bM[i], this.re, this.the);
rbt2.Update(this.rM[i], this.bM[i], this.re, this.the);
xf1.Add(rbt1.State[0, 0]);
yf1.Add(rbt1.State[2, 0]);
xf2.Add(rbt2.State[0, 0]);
yf2.Add(rbt2.State[2, 0]);
if (!ZeroCov.AlmostEqual((rbt2.Cov - rbt1.Cov), tolerance))
{
withinTolerance = false;
}
else if (!ZeroState.AlmostEqual((rbt2.State - rbt1.State), tolerance))
{
withinTolerance = false;
}
rbt1.Predict(this.T, this.q);
rbt2.Predict(this.T, this.q);
}
// Create strings
string x1s = ""; string y1s = ""; string x2s = ""; string y2s = "";
for (int i = 0; i < xf1.Count; i++)
{
x1s += xf1[i].ToString("#00.00") + "\t";
y1s += yf1[i].ToString("#00.00") + "\t";
x2s += xf2[i].ToString("#00.00") + "\t";
y2s += yf2[i].ToString("#00.00") + "\t";
}
System.Console.WriteLine("Filter 1 Estimates");
System.Console.WriteLine(x1s);
System.Console.WriteLine(y1s);
System.Console.WriteLine("Filter 2 Estimates");
System.Console.WriteLine(x2s);
System.Console.WriteLine(y2s);
return(withinTolerance);
}
private bool RunTest(IKalmanFilter filter1, IKalmanFilter filter2, double tolerance)
{
List<double> xf1 = new List<double>();
List<double> yf1 = new List<double>();
List<double> xf2 = new List<double>();
List<double> yf2 = new List<double>();
Matrix<double> ZeroCov = Matrix<double>.Build.Dense(filter1.Cov.RowCount, filter1.Cov.RowCount);
Matrix<double> ZeroState = Matrix<double>.Build.Dense(filter1.State.RowCount, 1);
RangeBearingTracker rbt1 = new RangeBearingTracker(filter1);
RangeBearingTracker rbt2 = new RangeBearingTracker(filter2);
bool withinTolerance = true;
// Predict the filters
rbt1.Predict(this.T, this.q);
rbt2.Predict(this.T, this.q);
for (int i = 2; i < this.bM.Length; i++)
{
rbt1.Update(this.rM[i], this.bM[i], this.re, this.the);
rbt2.Update(this.rM[i], this.bM[i], this.re, this.the);
xf1.Add(rbt1.State[0,0]);
yf1.Add(rbt1.State[2,0]);
xf2.Add(rbt2.State[0,0]);
yf2.Add(rbt2.State[2,0]);
if (!ZeroCov.AlmostEqual((rbt2.Cov - rbt1.Cov), tolerance))
withinTolerance = false;
else if (!ZeroState.AlmostEqual((rbt2.State - rbt1.State), tolerance))
withinTolerance = false;
rbt1.Predict(this.T, this.q);
rbt2.Predict(this.T, this.q);
}
// Create strings
string x1s = ""; string y1s = ""; string x2s = ""; string y2s = "";
for (int i=0; i < xf1.Count; i++)
{
x1s += xf1[i].ToString("#00.00") + "\t";
y1s += yf1[i].ToString("#00.00") + "\t";
x2s += xf2[i].ToString("#00.00") + "\t";
y2s += yf2[i].ToString("#00.00") + "\t";
}
System.Console.WriteLine("Filter 1 Estimates");
System.Console.WriteLine(x1s);
System.Console.WriteLine(y1s);
System.Console.WriteLine("Filter 2 Estimates");
System.Console.WriteLine(x2s);
System.Console.WriteLine(y2s);
return withinTolerance;
}
