/// <summary>
/// 自己位置推定計算
/// </summary>
/// <param name="LRF_Data">ＬＲＦ実取得データ</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;
}
/// <summary>
/// 自己位置推定計算
/// </summary>
/// <param name="LRF_Data">ＬＲＦ実取得データ</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;
}
/// <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));
}