Exemplo n.º 1
0
        /// <summary>
        ///   According to
        ///   http://www.swarthmore.edu/NatSci/echeeve1/Ref/Kalman/Ex2ScalarKalman.html
        /// </summary>
        public void FirstOrderOutputTest()
        {
            try
            {
                if (Chart == null)
                {
                    return;
                }

                // Add test Lines to demonstrate the control
                Chart.Primitives.Clear();

                const float a = 0.85f, b = 0, h = 3, Q = 0.1f, R = 1;
                var         filter   = new ScalarKF(a, b, h, Q, R);
                float[]     inValues = SwordfishGraphHelper.CreateFloatArray(100, 1);
                IList <StepOutput <float> > outValues = filter.Filter(inValues);

                var originalZValues = new List <float>(outValues.Select(element => element.Z));
//            var smoothedZValues = new List<float>();
                int halfWindowLength = inValues.Length / 10;
                for (int i = halfWindowLength; i < originalZValues.Count - halfWindowLength; i++)
                {
                    float windowMean = 0;
                    for (int j = -halfWindowLength; j <= halfWindowLength; j++)
                    {
                        int windowIndex = i + j;
                        windowMean += originalZValues[windowIndex];
                    }

                    windowMean /= halfWindowLength * 2;
                    //                smoothedZValues.Add(windowMean);
                    originalZValues[i] = windowMean;
                }

//            AddLineXY("N-Window Smoothing", Colors.DarkGoldenrod, originalZValues);//smoothedZValues);
//            AddLineXY("Noise W", Colors.White, outValues.Select(element => element.W));
//            AddLineXY("Noise V", Colors.LightGray, outValues.Select(element => element.V));
//            AddLineXY("k", Colors.Cyan, outValues.Select(element => element.K));
                SwordfishGraphHelper.AddLineXY(Chart, "A Posteriori X", Colors.YellowGreen,
                                               outValues.Select(element => element.PostX));
                SwordfishGraphHelper.AddLineXY(Chart, "A Priori X", Colors.Blue,
                                               outValues.Select(element => element.PriX));
                SwordfishGraphHelper.AddLineXY(Chart, "X", Colors.Red, outValues.Select(element => element.X));

                Chart.Title      = "FirstOrderOutputTest()";
                Chart.XAxisLabel = "X Axis";
                Chart.YAxisLabel = "Y Axis";

                Chart.RedrawPlotLines();
            }
            catch (Exception e)
            {
                MessageBox.Show(e.ToString());
            }
        }
        /// <summary></summary>
        public void HorizontalFlightTest(IEnumerable <HelicopterLogSnapshot> flightLog)
        {
            if (flightLog == null)
            {
                return;
            }

            if (Chart == null)
            {
                return;
            }


            // Add test Lines to demonstrate the control
            Chart.Primitives.Clear();

            IEnumerable <Vector3> horizontalGPSSamples = flightLog.Select(e => e.Observed.Position).Where(p => p != Vector3.Zero);

            float radiusVariance = new Vector3(
                horizontalGPSSamples.Select(e => e.X).StandardDeviation(),
                horizontalGPSSamples.Select(e => e.Y).StandardDeviation(),
                horizontalGPSSamples.Select(e => e.Z).StandardDeviation()).Length();

            Console.WriteLine(@"Radius std. dev. of GPS data: " + radiusVariance + @"m.");

            // GPS observations only occur every 1 second, so ignore Vector3.Zero positions (not 100% safe, but close enough)
            SwordfishGraphHelper.AddLineXZ(Chart, "GPS", Colors.Orange, horizontalGPSSamples);
            SwordfishGraphHelper.AddLineXZ(Chart, "Estimated", Colors.Navy, flightLog.Select(e => e.Estimated.Position));
            SwordfishGraphHelper.AddLineXZ(Chart, "True", Colors.DarkRed, flightLog.Select(e => e.True.Position));

            Chart.Title      = "HorizontalFlightTest()";
            Chart.XAxisLabel = "X [m]";
            Chart.YAxisLabel = "Z [m]";

            Chart.RedrawPlotLines();
        }
        /// <summary></summary>
        public void GPSFilter2DTest()
        {
            try
            {
                if (Chart == null)
                {
                    return;
                }

                // Add test Lines to demonstrate the control
                Chart.Primitives.Clear();

                var filterStartState = new GPSObservation
                {
                    Position = new Vector3(0, 0, 0),
                    //                                               Velocity = new Vector3(1, 0, 0),
                    //                                           Acceleration = new Vector3(0, 1, 0),
                    Time = TimeSpan.Zero,
                };

                var filter   = new GPSFilter2D(filterStartState);
                var inValues = new List <GPSObservation>();
                for (int t = 0; t < 20; t++)
                {
                    var obs = new GPSObservation
                    {
                        Position = new Vector3(t, 2.5f * 0.01f * t * t, 0),
                        //(float) (1 + 1*Math.Cos(2*Math.PI*4*t/100)), 0),
                        Time = TimeSpan.FromSeconds(t)
                    };


                    inValues.Add(obs);
                }


                var outValues = new List <StepOutput <GPSFilter2DSample> >();
                foreach (GPSObservation observation in inValues)
                {
                    outValues.Add(filter.Filter(observation, new GPSFilter2DInput
                    {
                        Velocity     = new Vector3(1, 0, 0),
                        Acceleration = new Vector3(0, 0.01f, 0),
                    }));
                }

//                IEnumerable<StepOutput<GPSFilter2DSample>> outValues = filter.Filter(inValues);

                //            AddLineXY("N-Window Smoothing", Colors.DarkGoldenrod, originalZValues);//smoothedZValues);
                //            AddLineXY("Noise W", Colors.White, outValues.Select(element => element.W));
                //                AddLineXY("Noise V", Colors.LightGray, outValues.Select(e => e.V.Position));
                //            AddLineXY("k", Colors.Cyan, outValues.Select(element => element.K));
                SwordfishGraphHelper.AddLineXY(Chart, "A Priori X", Colors.Blue, outValues.Select(e => e.PriX.Position));
                SwordfishGraphHelper.AddLineXY(Chart, "A Posteriori X", Colors.YellowGreen, outValues.Select(e => e.PostX.Position));
                SwordfishGraphHelper.AddLineXY(Chart, "X", Color.FromArgb(255, 150, 0, 0), outValues.Select(e => e.X.Position));
                SwordfishGraphHelper.AddLineXY(Chart, "Z", Color.FromArgb(255, 255, 255, 0), outValues.Select(e => e.Z.Position));
                //                AddLineXY(Chart, "True", Color.FromArgb(50, 150, 0, 0), inValues.Select(e => e.Position));

                Chart.Title      = "Filter2DTest()";
                Chart.XAxisLabel = "X [m]";
                Chart.YAxisLabel = "Y [m]";

                Chart.RedrawPlotLines();
            }
            catch (Exception e)
            {
                MessageBox.Show(e.ToString());
            }
        }
Exemplo n.º 4
0
        public void GaussianDistributionTest()
        {
            try
            {
                if (Chart == null)
                {
                    return;
                }

                try
                {
                    Chart.Primitives.Clear();

                    var rand = new GaussianRandom();

                    float minY   = float.MaxValue;
                    float maxY   = float.MinValue;
                    var   values = new float[100000];
                    for (int x = 0; x < values.Length; x++)
                    {
                        float val = rand.NextGaussian(0, 1);
                        values[x] = val;
                        if (val > maxY)
                        {
                            maxY = val;
                        }
                        if (val < minY)
                        {
                            minY = val;
                        }
                    }


                    const int distributionSteps       = 1000;
                    float     stepSizeY               = (maxY - minY) / distributionSteps;
                    var       distributionBucketCount = new Point[distributionSteps];

                    // Set X-positions for all points
                    for (int i = 0; i < distributionBucketCount.Length; i++)
                    {
                        distributionBucketCount[i].X = MyMathHelper.Lerp(i, 0, distributionBucketCount.Length, 0,
                                                                         values.Length);
                    }

                    // Increase Y-position by one for each bucket hit
                    foreach (float valY in values)
                    {
                        int bucketIndex = Convert.ToUInt16(Math.Min(distributionSteps - 1, (valY - minY) / stepSizeY));
                        distributionBucketCount[bucketIndex].Y++;
                    }

                    SwordfishGraphHelper.AddLineXY(Chart, "Gaussian values", Colors.Gray, values);
                    SwordfishGraphHelper.AddLineXY(Chart, "Distribution bucket count", Colors.Red,
                                                   distributionBucketCount);

                    Chart.Title      = "GaussianDistributionTest()";
                    Chart.XAxisLabel = "X Axis";
                    Chart.YAxisLabel = "Y Axis";

                    Chart.RedrawPlotLines();
                }
                catch (Exception e)
                {
                    MessageBox.Show(e.ToString());
                    Console.WriteLine(e);
                }
            }
            catch (Exception e)
            {
                MessageBox.Show(e.ToString());
            }
        }