/// <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 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()); } }
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()); } }