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