/// <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="stWldX">マップ座標X</param>
/// <param name="stWldY">マップ座標Y</param>
/// <param name="stDir">角度</param>
public void SetStartPostion(int stWldX, int stWldY, double stDir)
{
startMapX = stWldX;
startMapY = stWldY;
startMapDir = stDir;
MRF = new MapRangeFinder(LRF_Ctrl.LRFmaxRange_mm / MapToRealScale); // 仮想Map用 LRFクラス
worldMap.UpdateAreaCenter(stWldX, stWldY);
AreaBmp = worldMap.AreaGridMap.UpdateBitmap();
//bAreaMapUpdateReqest = false;
MRF.SetMap(worldMap.AreaGridMap);
// ロボットマーカ設定
R1 = new MarkPoint(stWldX, stWldY, stDir); // 実ロボット位置 R.E,Compass,GPSでの位置+ LRF補正
E1 = new MarkPoint(stWldX, stWldY, stDir); // R.Encoderの位置
oldE1 = new MarkPoint(stWldX, stWldY, stDir); //
E2 = new MarkPoint(stWldX, stWldY, stDir); // R.E Pluse & Compus の位置
V1 = new MarkPoint(stWldX, stWldY, stDir); // 推定位置ロボット
C1 = new MarkPoint(0, 0, stDir);
G1 = new MarkPoint(0, 0, 0); // GPS
oldG1 = new MarkPoint(stWldX, stWldY, stDir); //
A1 = new MarkPoint(0, 0, 0); // AMCL
// パーティクルフィルター初期化
{
// サンプル数(ParticleSizeと同じで、すべてのパーティクルを対象とする)
// LRFの有効距離を半径に変換(/2.0)、20%の距離で散らばる
// +-5度の範囲
ParticleFilter = new ParticleFilter(ParticleSize, (((LRF_Ctrl.LRFmaxRange_mm / 2.0) * 0.20) / MapToRealScale), 30.0); // サンプリング数、パーティクルのレンジ
Particles = new List <Particle>();
for (int i = 0; i < ParticleSize; ++i)
{
// パーティクルマーカー
Particles.Add(new Particle(new MarkPoint(0, 0, 0), 0));
}
}
// ローパスフィルター初期化
ResetLPF_V1(V1);
}
/// <summary>
/// MAP初期化
/// </summary>
/// <param name="fname"></param>
public void MapInit(Bitmap mapBmp)
{
// 30m
mrf = new MapRangeFinder((30 * 1000), PixelScale, mapBmp);
}
/// <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="stWldX">マップ座標X</param>
/// <param name="stWldY">マップ座標Y</param>
/// <param name="stDir">角度</param>
public void SetStartPostion(int stWldX, int stWldY, double stDir)
{
MRF = new MapRangeFinder(LRF_Ctrl.LRFmaxRange_mm / MapToRealScale); // 仮想Map用 LRFクラス
worldMap.UpdateAreaCenter(stWldX, stWldY);
AreaBmp = worldMap.AreaGridMap.UpdateBitmap();
//bAreaMapUpdateReqest = false;
MRF.SetMap(worldMap.AreaGridMap);
// ロボットマーカ設定
R1 = new MarkPoint(stWldX, stWldY, stDir); // 実ロボット位置 R.E,Compass,GPSでの位置+ LRF補正
E1 = new MarkPoint(stWldX, stWldY, stDir); // R.Encoderの位置
oldE1 = new MarkPoint(stWldX, stWldY, stDir); //
E2 = new MarkPoint(stWldX, stWldY, stDir); // R.E Pluse & Compus の位置
V1 = new MarkPoint(stWldX, stWldY, stDir); // 推定位置ロボット
C1 = new MarkPoint(0, 0, stDir);
G1 = new MarkPoint(0, 0, 0); // GPS
oldG1 = new MarkPoint(stWldX, stWldY, stDir); //
// パーティクルフィルター初期化
{
// サンプル数(ParticleSizeと同じで、すべてのパーティクルを対象とする)
// LRFの有効距離を半径に変換(/2.0)、20%の距離で散らばる
// +-5度の範囲
ParticleFilter = new ParticleFilter(ParticleSize, (((LRF_Ctrl.LRFmaxRange_mm / 2.0) * 0.20) / MapToRealScale), 30.0 ); // サンプリング数、パーティクルのレンジ
Particles = new List<Particle>();
for (int i = 0; i < ParticleSize; ++i)
{
// パーティクルマーカー
Particles.Add(new Particle(new MarkPoint(0,0,0), 0));
}
}
// ローパスフィルター初期化
ResetLPF_V1(V1);
}
