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