コード例 #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);
            }
        }
コード例 #2
0
        /// <summary>
        /// PF自己位置計算 継続
        /// </summary>
        /// <param name="mkp"></param>
        /// <param name="useLowFilter"></param>
        /// <param name="numPF"></param>
        public void ParticleFilterLocalize(bool useLPF)
        {
            MarkPoint locMkp = new MarkPoint(worldMap.GetAreaX(V1.X), worldMap.GetAreaY(V1.Y), V1.Theta);

            // パーティクルフィルター演算
            ParticleFilter.Localize(LRF.getData(), MRF, locMkp, Particles);

            if (useLPF)
            {
                // 結果にローパスフィルターをかける
                V1.X     = lpfV1X.update(worldMap.GetWorldX(locMkp.X));
                V1.Y     = lpfV1Y.update(worldMap.GetWorldY(locMkp.Y));
                V1.Theta = lpfV1Theta.update(locMkp.Theta);
            }
            else
            {
                V1.X     = worldMap.GetWorldX(locMkp.X);
                V1.Y     = worldMap.GetWorldY(locMkp.Y);
                V1.Theta = locMkp.Theta;
            }
        }