コード例 #1
0
        /// <summary>
        /// 自己位置推定計算
        /// </summary>
        /// <param name="LRF_Data">LRF実取得データ</param>
        /// <param name="MRF">MAPデータ</param>
        /// <param name="mkp">想定ロボット位置(計算基点)</param>
        /// <param name="Particles">パーティクル作業バッファ</param>
        public void Localize(double[] LRF_Data, MapRangeFinder MRF, MarkPoint mkp, List <Particle> Particles)
        {
            double sum = 0;

            if (null == LRF_Data)
            {
                Console.WriteLine("ParticleFilter.cs Localize() : Error! null LrfData");
                return;
            }

            // パーティクルをばらまく
            //for (int i = 0; i < Particles.Count; ++i)
            int numParticles = Particles.Count;

            Parallel.For(0, numParticles, i =>

            {
                // 散らばらせる
                MakeParticle(mkp, PtclRange, PtclDirRange, Particles[i].Location);

                // 散らばり% = w ?
                // マップデータとLRFのデータを比べる
                Particles[i].W = Likefood(LRF_Data, MRF.Sense(Particles[i].Location));
                sum           += Particles[i].W;
            });

            // W順に並べ替え
            Particles.Sort((a, b) => (int)((a.W - b.W) * 1000.0));

            // パーティクルから 整合性の高いデータを取り出す?(リサンプリング)
            for (int n = 0; n < ResamplingNumber; ++n)
            {
                double selectVal = Rand.NextDouble() * sum;
                double subsum    = 0;
                for (int i = 0; i < Particles.Count; ++i)
                {
                    subsum += Particles[i].W;
                    if (selectVal < subsum)
                    {
                        ResamplingSet[n] = Particles[i].Location;
                        break;
                    }
                }
            }

            // パーティクルの平均値から、仮想ロボットの位置を補正
            double newX     = 0;
            double newY     = 0;
            double newTheta = 0;

            for (int n = 0; n < ResamplingNumber; ++n)
            {
                newX     += ResamplingSet[n].X;
                newY     += ResamplingSet[n].Y;
                newTheta += ResamplingSet[n].Theta;
            }

            mkp.X     = newX / ResamplingNumber;
            mkp.Y     = newY / ResamplingNumber;
            mkp.Theta = newTheta / ResamplingNumber;
        }
コード例 #2
0
        /// <summary>
        /// 自己位置推定計算
        /// </summary>
        /// <param name="LRF_Data">LRF実取得データ</param>
        /// <param name="MRF">MAPデータ</param>
        /// <param name="mkp">想定ロボット位置(計算基点)</param>
        /// <param name="Particles">パーティクル作業バッファ</param>
        public void Localize(double[] LRF_Data, MapRangeFinder MRF, MarkPoint mkp, List<Particle> Particles)
        {
            double sum = 0;

            if (null == LRF_Data)
            {
                Console.WriteLine("ParticleFilter.cs Localize() : Error! null LrfData");
                return;
            }

            // パーティクルをばらまく
            //for (int i = 0; i < Particles.Count; ++i)
            int numParticles = Particles.Count;
            Parallel.For(0, numParticles, i =>

                {
                    // 散らばらせる
                    MakeParticle(mkp, PtclRange, PtclDirRange, Particles[i].Location);

                    // 散らばり% = w ?
                    // マップデータとLRFのデータを比べる
                    Particles[i].W = Likefood(LRF_Data, MRF.Sense(Particles[i].Location));
                    sum += Particles[i].W;
                });

            // W順に並べ替え
            Particles.Sort((a, b) => (int)((a.W - b.W)*1000.0));

            // パーティクルから 整合性の高いデータを取り出す?(リサンプリング)
            for (int n = 0; n < ResamplingNumber; ++n)
            {
                double selectVal = Rand.NextDouble() * sum;
                double subsum = 0;
                for (int i = 0; i < Particles.Count; ++i)
                {
                    subsum += Particles[i].W;
                    if (selectVal < subsum)
                    {
                        ResamplingSet[n] = Particles[i].Location;
                        break;
                    }
                }
            }

            // パーティクルの平均値から、仮想ロボットの位置を補正
            double newX = 0;
            double newY = 0;
            double newTheta = 0;
            for (int n = 0; n < ResamplingNumber; ++n) {
                newX += ResamplingSet[n].X;
                newY += ResamplingSet[n].Y;
                newTheta += ResamplingSet[n].Theta;
            }

            mkp.X = newX / ResamplingNumber;
            mkp.Y = newY / ResamplingNumber;
            mkp.Theta = newTheta / ResamplingNumber;
        }
コード例 #3
0
        /// <summary>
        /// 指定位置のマップ障害物情報をLRFの値として返す
        /// </summary>
        /// <param name="mapX"></param>
        /// <param name="mapY"></param>
        /// <param name="ang"></param>
        /// <returns></returns>
        public double[] GetMapLRF(int mapX, int mapY, double ang)
        {
            MarkPoint mkp = new MarkPoint(worldMap.GetAreaX(mapX), worldMap.GetAreaY(mapY), ang);

            return(MRF.Sense(mkp));
        }