Esempio n. 1
0
        public MatrixKalmanWrapper()
        {
            /*
             * X0 : predicted state
             * P0 : predicted covariance
             *
             * F : factor of real value to previous real value
             * B : the control-input model which is applied to the control vector uk;
             * U : the control-input model which is applied to the control vector uk;
             * Q : measurement noise
             * H : factor of measured value to real value
             * R : environment noise
             */
            var f = new Matrix(new[, ] {
                { 1.0, 1 }, { 0, 1.0 }
            });
            var b = new Matrix(new[, ] {
                { 0.0 }, { 0 }
            });
            var u = new Matrix(new[, ] {
                { 0.0 }, { 0 }
            });
            var r = Matrix.CreateVector(10);
            var q = new Matrix(new[, ] {
                { 0.01, 0.4 }, { 0.1, 0.02 }
            });
            var h = new Matrix(new[, ] {
                { 1.0, 0 }
            });

            kX = makeKalmanFilter(f, b, u, q, h, r);
            kY = makeKalmanFilter(f, b, u, q, h, r);
            kZ = makeKalmanFilter(f, b, u, q, h, r);
        }
Esempio n. 2
0
        public MatrixKalmanWrapper(double set_q)
        {
            var Ts = 0.02;
            var Q  = set_q;
            var G  = Matrix.CreateVector(Mathf.Pow((float)Ts, 3) / 6, Mathf.Pow((float)Ts, 3) / 2, Ts);
            var a  = new Matrix(new[, ] {
                { 0, 1.0, 0 }, { 0, 0, 1.0 }, { 0, 0, 0 }
            });                                                                               //a

            /*
             *          X0 : predicted state
             *          P0 : predicted covariance
             *
             *          F : factor of real value to previous real value
             *          B : the control-input model which is applied to the control vector uk;
             *          U : the control-input model which is applied to the control vector uk;
             *          Q : measurement noise
             *          H : factor of measured value to real value
             *          R : environment noise
             */
            var f      = Matrix.IdentityMatrix(3) + (Ts * a) + 0.5 * Matrix.Power(Ts * a, 2); //ad
            var b      = Matrix.CreateVector(0);
            var u      = Matrix.CreateVector(0);
            var r_good = Matrix.CreateVector(0.68);
            var r_bad  = Matrix.CreateVector(1000000000000);
            var q      = (Q * G) * G.Transpose();                                                   //g * q * g.T
            var h      = new Matrix(new[, ] {
                { 1.0, 0, 0 }
            });                                                                                         //c

            kY = makeKalmanFilter(f, b, u, q, h, r_good, r_bad);
        }
        private void WindowLoaded(object sender, RoutedEventArgs e)
        {
            var temp = new PlotModel();

            var rs = new LineSeries();
            rs.StrokeThickness = 1;
            var fuelData = File.ReadAllLines(@"Data/Fuel.txt").Select(double.Parse).ToList();
            for (int i = 0; i < fuelData.Count; i++) rs.Points.Add(new DataPoint(i, fuelData[i]));
            temp.Series.Add(rs);

            var f = new Matrix(new[,] {{1.0, 1}, {0, 1}});
            var b = new Matrix(new[,] {{0.0}, {0}});
            var u = new Matrix(new[,] {{0.0}, {0}});
            var r = Matrix.CreateVector(10);
            var q = new Matrix(new[,] {{0.01, 0.4}, {0.1, 0.02}});
            var h = new Matrix(new[,] {{1.0 , 0}});

            var kalman = new KalmanFilter(f, b, u, q, h, r); // задаем F, H, Q и R
            kalman.SetState(Matrix.CreateVector(fuelData[0], 0), new Matrix(new[,] {{10.0, 0}, {0 , 10.0}})); // задаем начальные State и Covariance

            var filtered = new List<double>();
            var rashod = new List<double>();
            foreach (var d in fuelData)
            {
                try
                {
                    kalman.Correct(new Matrix(new[,] {{d}}));
                    filtered.Add(kalman.State[0, 0]);
                    rashod.Add(kalman.State[1, 0]);
                }
                catch(Exception ex)
                {
                    MessageBox.Show(ex.Message);
                }
            }

            var fs = new LineSeries();
            for (int i = 0; i < filtered.Count; i++) fs.Points.Add(new DataPoint(i, filtered[i]));
            temp.Series.Add(fs);

            var ras = new LineSeries();
            for (int i = 0; i < filtered.Count; i++) ras.Points.Add(new DataPoint(i, rashod[i]));
            temp.Series.Add(ras);

            temp.Axes.Add(new LinearAxis(AxisPosition.Left, 0, 4200));
            temp.Axes.Add(new LinearAxis(AxisPosition.Bottom));
            Plot.Model = temp;
        }
Esempio n. 4
0
 KalmanFilter makeKalmanFilter(Matrix f, Matrix b, Matrix u, Matrix q, Matrix h, Matrix r)
 {
     var filter = new KalmanFilter (
         f.Duplicate (),
         b.Duplicate (),
         u.Duplicate (),
         q.Duplicate (),
         h.Duplicate (),
         r.Duplicate ()
     );
     // set initial value
     filter.SetState (
         Matrix.CreateVector (500, 0),
         new Matrix (new [,] {{10.0, 0}, {0, 5.0}})
     );
     return filter;
 }
Esempio n. 5
0
        public MatrixKalmanWrapper()
        {
            /*
             * X0 : predicted state
             * P0 : predicted covariance
             *
             * F : factor of real value to previous real value
             * B : the control-input model which is applied to the control vector uk;
             * U : the control-input model which is applied to the control vector uk;
             * Q : measurement noise
             * H : factor of measured value to real value
             * R : environment noise
             */
            var ts = 0.033;
            var f  = new Matrix(new[, ] {
                { 1.0, ts, 0 }, { 0, 1, 0 }, { 0, 0, 1 }
            });
            var b = new Matrix(new[, ] {
                { 0.0 }, { 0 }
            });
            var u = new Matrix(new[, ] {
                { 0.0 }, { 0 }
            });
            //var r = Matrix.CreateVector (10);
            //var q = new Matrix (new[,] {{0.01, 0.4}, {0.1, 0.02}});
            var h = new Matrix(new[, ] {
                { 1.0, 0, 0 }, { 1, 0, 0 }, { 0, 1, -1 }, { 0, 1, -1 }
            });
            var r = new Matrix(new[, ] {
                { 1.0, 0, 0, 0 }, { 0, 1, 0, 0 }, { 0, 0, 1, 0 }, { 0, 0, 0, 1 }
            });

            var gd = new Matrix(new[, ] {
                { 1 / 2 * Mathf.Pow((float)ts, 2), 0 }, { ts, 0 }, { 0, ts }
            });
            var q_raw = new Matrix(new[, ] {
                { 100, 0 }, { 0, 10.0 }
            });

            var q = gd * q_raw * gd.Transpose();

            kX = makeKalmanFilter(f, b, u, q, h, r);
            kY = makeKalmanFilter(f, b, u, q, h, r);
            kZ = makeKalmanFilter(f, b, u, q, h, r);
        }
Esempio n. 6
0
        KalmanFilter makeKalmanFilter(Matrix f, Matrix b, Matrix u, Matrix q, Matrix h, Matrix r)
        {
            var filter = new KalmanFilter(
                f.Duplicate(),
                b.Duplicate(),
                u.Duplicate(),
                q.Duplicate(),
                h.Duplicate(),
                r.Duplicate()
                );

            // set initial value
            filter.SetState(
                Matrix.CreateVector(500, 0),
                new Matrix(new [, ] {
                { 10.0, 0 }, { 0, 5.0 }
            })
                );
            return(filter);
        }
Esempio n. 7
0
        public MatrixKalmanWrapper()
        {
            /*
            X0 : predicted state
            P0 : predicted covariance

            F : factor of real value to previous real value
            B : the control-input model which is applied to the control vector uk;
            U : the control-input model which is applied to the control vector uk;
            Q : measurement noise
            H : factor of measured value to real value
            R : environment noise
            */
            var f = new Matrix (new[,] {{1.0, 1}, {0, 1.0}});
            var b = new Matrix (new[,] {{0.0}, {0}});
            var u = new Matrix (new[,] {{0.0}, {0}});
            var r = Matrix.CreateVector (10);
            var q = new Matrix (new[,] {{0.01, 0.4}, {0.1, 0.02}});
            var h = new Matrix (new[,] {{1.0 , 0}});

            kX = makeKalmanFilter (f, b, u, q, h, r);
            kY = makeKalmanFilter (f, b, u, q, h, r);
            kZ = makeKalmanFilter (f, b, u, q, h, r);
        }
Esempio n. 8
0
        private void WindowLoaded(object sender, RoutedEventArgs e)
        {
            var temp = new PlotModel();


            var rs = new LineSeries();

            rs.StrokeThickness = 1;
            var fuelData = File.ReadAllLines(@"Data/Fuel.txt").Select(double.Parse).ToList();

            for (int i = 0; i < fuelData.Count; i++)
            {
                rs.Points.Add(new DataPoint(i, fuelData[i]));
            }
            temp.Series.Add(rs);

            var f = new Matrix(new[, ] {
                { 1.0, 1 }, { 0, 1 }
            });
            var b = new Matrix(new[, ] {
                { 0.0 }, { 0 }
            });
            var u = new Matrix(new[, ] {
                { 0.0 }, { 0 }
            });
            var r = Matrix.CreateVector(10);
            var q = new Matrix(new[, ] {
                { 0.01, 0.4 }, { 0.1, 0.02 }
            });
            var h = new Matrix(new[, ] {
                { 1.0, 0 }
            });

            var kalman = new KalmanFilter(f, b, u, q, h, r); // задаем F, H, Q и R

            kalman.SetState(Matrix.CreateVector(fuelData[0], 0), new Matrix(new[, ] {
                { 10.0, 0 }, { 0, 10.0 }
            }));                                                                                              // задаем начальные State и Covariance

            var filtered = new List <double>();
            var rashod   = new List <double>();

            foreach (var d in fuelData)
            {
                try
                {
                    kalman.Correct(new Matrix(new[, ] {
                        { d }
                    }));
                    filtered.Add(kalman.State[0, 0]);
                    rashod.Add(kalman.State[1, 0]);
                }
                catch (Exception ex)
                {
                    MessageBox.Show(ex.Message);
                }
            }

            var fs = new LineSeries();

            for (int i = 0; i < filtered.Count; i++)
            {
                fs.Points.Add(new DataPoint(i, filtered[i]));
            }
            temp.Series.Add(fs);

            var ras = new LineSeries();

            for (int i = 0; i < filtered.Count; i++)
            {
                ras.Points.Add(new DataPoint(i, rashod[i]));
            }
            temp.Series.Add(ras);

            temp.Axes.Add(new LinearAxis(AxisPosition.Left, 0, 4200));
            temp.Axes.Add(new LinearAxis(AxisPosition.Bottom));
            Plot.Model = temp;
        }