/// <summary>
/// 任意座標でのＰＦ補正
/// </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);
}
}