/// <summary>
/// Form a test.
/// </summary>
/// <param name="idealNoNoise"></param>
protected void formTest(double [] idealNoNoise)
{
Random.Random r = new Random.Random();
// Add in wind, auto-pilot on our rocket always successfully adapts so
// that the effect by the wind is guassian process noise.
double[] withProcessNoise = new double[idealNoNoise.Length];
for (int i = 0; i < idealNoNoise.Length; i++)
{
// Add in 1 foot = 1 std. deviation of process noise.
withProcessNoise[i] = idealNoNoise[i] + r.NextGuass_Double(0, (double)numericUpDown5.Value);
}
// Add in measurement noise, the GPS on the rocket measures less often
// than our data but we can just skip things.
// Assume a very good GPS, accurate to +/- 6 feet even at these speeds.
// 2 foot = 1 std dev. is about +/- 6 feet
double[] withAllNoise = new double[idealNoNoise.Length];
for (int i = 0; i < idealNoNoise.Length; i++)
{
// Add in 1 foot = 1 std. deviation of process noise.
withAllNoise[i] = withProcessNoise[i] + r.NextGuass_Double(0, (double)numericUpDown6.Value);
}
Kalman1D k = new Kalman1D();
k.Reset(
(double)numericUpDown1.Value,
(double)numericUpDown2.Value,
(double)numericUpDown3.Value,
(double)numericUpDown4.Value, 0);
// Assume we get to see every other measurement we calculated, and use
// the others as the points to compare for estimates.
// Run the filter, note our time unit is 1.
double[] kalman = new double[idealNoNoise.Length];
double[] vel = new double[idealNoNoise.Length];
double[] kGain = new double[idealNoNoise.Length];
for (int i = 0; i < idealNoNoise.Length; i += 2)
{
if (i == 0)
{
kalman[0] = 0;
vel[0] = k.Velocity;
kGain[0] = k.LastGain;
kalman[1] = k.Predicition(1);
vel[1] = k.Velocity;
kGain[1] = k.LastGain;
}
else
{
kalman[i] = k.Update(withAllNoise[i], 1);
kalman[i + 1] = kalman[i];
vel[i] = k.Velocity;
kGain[i] = k.LastGain;
vel[i + 1] = vel[i];
kGain[i + 1] = kGain[i];
// kalman[i + 1] = k.Predicition(1);
}
}
// Compute errors.
double[] kalmanDelIdeal = new double[idealNoNoise.Length];
double[] measuredDelIdeal = new double[idealNoNoise.Length];
for (int i = 0; i < idealNoNoise.Length; i += 2)
{
kalmanDelIdeal[i] = kalman[i] - idealNoNoise[i];
measuredDelIdeal[i] = withAllNoise[i] - idealNoNoise[i];
}
// Chart stuff.
chart1.Series.Clear();
graph(chart1, "ideal", idealNoNoise, Color.Black);
graph(chart1, "measured", withAllNoise, Color.Blue);
graph(chart1, "kalman", kalman, Color.Red);
chart1.ChartAreas[0].RecalculateAxesScale();
chart2.Series.Clear();
graph(chart2, "Measurement", measuredDelIdeal, Color.Blue);
graph(chart2, "Kalman", kalmanDelIdeal, Color.Black);
chart2.ChartAreas[0].RecalculateAxesScale();
chart3.Series.Clear();
graph(chart3, "Velocity", vel, Color.Black);
chart3.ChartAreas[0].RecalculateAxesScale();
chart4.Series.Clear();
graph(chart4, "Gain", kGain, Color.Black);
chart4.ChartAreas[0].RecalculateAxesScale();
}
