/// <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); } }