示例#1
0
        public MainWindowViewModel()
        {
            Measurements = new ObservableCollection <Measurement>();
            Estimates    = new ObservableCollection <Measurement>();

            var filter = new UKF();
            var N      = 100;

            for (int k = 1; k < N; k++)
            {
                double[] z = ProcessBuilder.SineWave(k);
                filter.Update(z);
                var state      = filter.getState();
                var covariance = filter.getCovariance();

                Measurements.Add(new Measurement()
                {
                    Value = z[0], Time = TimeSpan.FromSeconds(k)
                });
                Estimates.Add(new Measurement()
                {
                    Value = state[0], Time = TimeSpan.FromSeconds(k), Variance = covariance[0, 0]
                });
            }
        }
示例#2
0
        static void Main(string[] args)
        {
            var filter = new UKF(1, 1);

            var n = 1;                                  //number of state
            var q = 0.05;                               //std of process
            var r = 0.3;                                //std of measurement
            var Q = Matrix.Build.Diagonal(n, n, q * q); //covariance of process
            var R = Matrix.Build.Dense(1, 1, r * r);    //covariance of measurement
            var f = new FEquation();                    //nonlinear state equations
            var h = new HEquation();                    //measurement equation
            var x = q * Matrix.Build.Random(1, 1);      //s + q * Matrix.Build.Random(1, 1); //initial state with noise
            var P = Matrix.Build.Diagonal(n, n, 1);     //initial state covraiance
            var N = 100;                                //total dynamic steps

            var xV = Matrix.Build.Dense(n, N, 0);       //estmate
            var zV = Matrix.Build.Dense(1, N, 0);       //measurement

            for (int k = 1; k < N; k++)
            {
                var z = Matrix.Build.Dense(1, 1, Math.Sin(k * 3.14 * 5 / 180)).Add(Matrix.Build.Random(1, 1).Multiply(r)); //measurments
                zV.SetSubMatrix(0, k, z);                                                                                  //save measurment
                var x_and_P = filter.Update(f, x, P, h, z, Q, R);                                                          //ukf
                x = x_and_P[0];
                P = x_and_P[1];
                xV.SetColumn(k, x.Column(0).ToArray());                          //save estimate
            }

            GraphPane     myPane  = new GraphPane(new RectangleF(0, 0, 3200, 2400), "Unscented Kalman Filter", "number", "measurement");
            PointPairList list_zV = new PointPairList();
            PointPairList list_xV = new PointPairList();

            for (int i = 0; i < zV.ColumnCount; i++)
            {
                list_zV.Add(i, zV[0, i]);
                list_xV.Add(i, xV[0, i]);
            }
            myPane.AddCurve("measurement", list_zV, Color.Red, SymbolType.Circle);
            myPane.AddCurve("estimate", list_xV, Color.Green, SymbolType.XCross);
            Bitmap   bm = new Bitmap(200, 200);
            Graphics g  = Graphics.FromImage(bm);

            myPane.AxisChange(g);
            Image im = myPane.Image;

            im.Save("result.png", ImageFormat.Png);
        }
示例#3
0
        /// <summary>
        /// UnscentedKalmanFilter 对一维数据进行处理
        /// </summary>
        /// <param name="data">使用引用传值方式修改data</param>
        public static void UnscentedKalmanFilterMethod(List <double> data)
        {
            var filter = new UKF();

            for (int i = 0; i < data.Count; i++)
            {
                filter.Update(new[] { data[i] });
                // filter.Update(new[] { data1[i], data2[i] });
                //filter.Update(new[] { data1[i], data2[i], data3[i] });
                try
                {
                    data[i] = filter.getState()[0];
                }
                catch
                {
                    System.Diagnostics.Debug.WriteLine("UnscentedKalmanFilterMethod:filter index 超出");
                }
            }
        }
示例#4
0
        public void Run()
        {
            var filter = new UKF(1, 1);

            n = 1;
            q = 0.05;
            r = 0.3;
            N = 100;

            Q = Matrix.Build.Diagonal(n, n, q * q); //covariance of process
            NotifyChanged("Q");
            R = Matrix.Build.Dense(1, 1, r * r);    //covariance of measurement
            f = new FEquation();                    //nonlinear state equations
            h = new HEquation();                    //measurement equation
            x = q * Matrix.Build.Random(1, 1);      //s + q * Matrix.Build.Random(1, 1); //initial state with noise
            P = Matrix.Build.Diagonal(n, n, 1);     //initial state covariance


            var xV = Matrix.Build.Dense(n, N, 0); //Estimate
            var zV = Matrix.Build.Dense(1, N, 0); //measurement


            for (int k = 1; k < N; k++)
            {
                Matrix <double> z = ProcessBuilder.SineWave(k, r);
                //measurments

                Matrix <double>[] x_and_P = filter.Update(f, x, P, h, z, Q, R);                //ukf
                x = x_and_P[0];
                P = x_and_P[1];

                Measurements.Add(new Measurement()
                {
                    Value = z[0, 0], Time = TimeSpan.FromSeconds(k)
                });
                Estimates.Add(new Measurement()
                {
                    Value = x_and_P[0][0, 0], Time = TimeSpan.FromSeconds(k), Variance = x_and_P[1][0, 0]
                });
            }
        }
示例#5
0
文件: Program.cs 项目: zk47/UKFSharp
        static void Main(string[] args)
        {
            var filter = new UKF();

            List <double> measurements = new List <double>();
            List <double> states       = new List <double>();

            Random rnd = new Random();

            for (int k = 0; k < 100; k++)
            {
                var measurement = Math.Sin(k * 3.14 * 5 / 180) + (double)rnd.Next(50) / 100;
                measurements.Add(measurement);
                filter.Update(new[] { measurement });
                states.Add(filter.getState()[0]);
            }

            GraphPane     myPane            = new GraphPane(new RectangleF(0, 0, 3200, 2400), "Unscented Kalman Filter", "number", "measurement");
            PointPairList measurementsPairs = new PointPairList();
            PointPairList statesPairs       = new PointPairList();

            for (int i = 0; i < measurements.Count; i++)
            {
                measurementsPairs.Add(i, measurements[i]);
                statesPairs.Add(i, states[i]);
            }
            myPane.AddCurve("measurement", measurementsPairs, Color.Red, SymbolType.Circle);
            myPane.AddCurve("estimate", statesPairs, Color.Green, SymbolType.XCross);
            Bitmap   bm = new Bitmap(200, 200);
            Graphics g  = Graphics.FromImage(bm);

            myPane.AxisChange(g);
            Image im = myPane.Image;

            im.Save("result.png", ImageFormat.Png);
        }