Ejemplo n.º 1
0
        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));
        }
Ejemplo n.º 2
0
        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 ZeroCov = new Matrix(filter1.Cov.RowCount, filter1.Cov.RowCount);
            Matrix ZeroState = new Matrix(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 (!Matrix.AlmostEqual(ZeroCov, (rbt2.Cov - rbt1.Cov), tolerance))
                    withinTolerance = false;
                else if (!Matrix.AlmostEqual(ZeroState, (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;
        }
Ejemplo n.º 3
0
        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        ZeroCov   = new Matrix(filter1.Cov.RowCount, filter1.Cov.RowCount);
            Matrix        ZeroState = new Matrix(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 (!Matrix.AlmostEqual(ZeroCov, (rbt2.Cov - rbt1.Cov), tolerance))
                {
                    withinTolerance = false;
                }
                else if (!Matrix.AlmostEqual(ZeroState, (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);
        }