/// <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);
}
}
private List<Particle> Particles; // �p�[�e�B�N�����X�g
#endregion Fields
#region Methods
/// <summary>
/// �C�Ӎ��W�ł̂o�e�
/// </summary>
/// <param name="numPF"></param>
public void CalcLocalize(MarkPoint mkp, bool useLowFilter, int numPF = 1)
{
// �p�[�e�B�N���t�B���^�[���Z
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);
// ���ʂɃ��[�p�X�t�B���^�[�������
if (null == lpfX)
{
// �ŏ��̒l������l�ɂ���
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);
}
}
/// <summary>
/// ローパスフィルター初期化
/// </summary>
/// <param name="mkp"></param>
public void ResetLPF_V1(MarkPoint mkp)
{
lpfV1X = new KalmanFilter.SimpleLPF(mkp.X);
lpfV1Y = new KalmanFilter.SimpleLPF(mkp.Y);
lpfV1Theta = new KalmanFilter.SimpleLPF(mkp.Theta);
}
/// <summary>
/// ローパスフィルター初期化
/// </summary>
/// <param name="mkp"></param>
public void ResetLPF_V1(MarkPoint mkp)
{
lpfV1X = new KalmanFilter.SimpleLPF(mkp.X);
lpfV1Y = new KalmanFilter.SimpleLPF(mkp.Y);
lpfV1Theta = new KalmanFilter.SimpleLPF(mkp.Theta);
}
