Ejemplo n.º 1
0
        /// <summary>
        /// 任意座標でのPF補正
        /// </summary>
        /// <param name="numPF"></param>
        public void CalcLocalize(MarkPoint mkp, bool useLowFilter, int numPF = 1)
        {
            // パーティクルフィルター演算

            if (useLowFilter)
            {
                KalmanFilter.SimpleLPF lpfX    = null;
                KalmanFilter.SimpleLPF lpfY    = null;
                KalmanFilter.SimpleLPF lpTheta = null;

                for (int i = 0; i < numPF; i++)
                {
                    MarkPoint locMkp = new MarkPoint(worldMap.GetAreaX(mkp.X), worldMap.GetAreaY(mkp.Y), mkp.Theta);

                    ParticleFilter.Localize(LRF.getData(), MRF, locMkp, Particles);

                    locMkp.X = worldMap.GetWorldX(locMkp.X);
                    locMkp.Y = worldMap.GetWorldY(locMkp.Y);

                    // 結果にローパスフィルターをかける
                    if (null == lpfX)
                    {
                        // 最初の値を初期値にする
                        lpfX    = new KalmanFilter.SimpleLPF(locMkp.X);
                        lpfY    = new KalmanFilter.SimpleLPF(locMkp.Y);
                        lpTheta = new KalmanFilter.SimpleLPF(locMkp.Theta);
                    }
                    else
                    {
                        lpfX.update(locMkp.X);
                        lpfY.update(locMkp.Y);
                        lpTheta.update(locMkp.Theta);
                    }
                }

                if (null != lpfX)
                {
                    mkp.X     = lpfX.value();
                    mkp.Y     = lpfY.value();
                    mkp.Theta = lpTheta.value();
                }
            }
            else
            {
                ParticleFilter.Localize(LRF.getData(), MRF, mkp, Particles);
            }
        }