示例#1
0
        /// <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();
        }