/// <summary>
/// 位置補正執行
/// </summary>
/// <param name="bRevisionFromGPS">GPSの値をそのまま使う</param>
public string LocalizeRevision(bool bRevisionFromGPS)
{
// 補正ログメッセージ
string revisionMsg = "";
// 補正後座標
MarkPoint rev = new MarkPoint(V1);
revisionMsg += "LocRevision:Issue / 座標補正を実行\n";
revisionMsg += "LocRevision: beforR1 R1.X" + R1.X.ToString("f2") + "/R1.Y " + R1.Y.ToString("f2") + "/R1.Dir " + R1.Theta.ToString("f2") + "\n";
// GPSが有効なら、GPSの値を元にPF補正
if (bEnableGPS)
{
// 補正基準 位置,向き
double RivLocX = G1.X;
double RivLocY = G1.Y;
double RivDir = (bActiveCompass ? C1.Theta : R1.Theta); // コンパスが使えるなら、コンパスの値を使う
if (bActiveCompass)
{
// コンパスを使った
revisionMsg += "LocRevision:useCompass C1.Dir " + C1.Theta.ToString("f2") + "\n";
}
if (bRevisionFromGPS)
{
// GPSの値をそのまま使う
rev.Set(RivLocX, RivLocY, RivDir);
// GPSを使った
revisionMsg += "LocRevision: useGPS G1.X" + G1.X.ToString("f2") + "/G1.Y " + G1.Y.ToString("f2") + "/G1.Dir " + G1.Theta.ToString("f2") + "\n";
}
else
{
// 補正基準位置からパーティクルフィルター補正
rev.Set(RivLocX, RivLocY, RivDir);
// V1ローパスフィルターリセット
ResetLPF_V1(rev);
// 自己位置推定演算 10回
CalcLocalize(V1, true, 10);
// PF
revisionMsg += "LocRevision: usePF V1.X" + V1.X.ToString("f2") + "/V1.Y " + V1.Y.ToString("f2") + "/V1.Dir " + V1.Theta.ToString("f2") + "\n";
}
}
else
{
// V1の値をそのまま使う
revisionMsg += "LocRevision: V1toR1 V1.X" + V1.X.ToString("f2") + "/ V1.Y " + V1.Y.ToString("f2") + "/ V1.Dir " + V1.Theta.ToString("f2") + "\n";
}
// 結果を反映
R1.Set(rev);
E1.Set(rev);
oldE1.Set(rev);
return(revisionMsg);
}
/// <summary>
/// ロータリーエンコーダ 現在位置取得
/// 差分を計算し現在位置更新
/// </summary>
private MarkPoint updateMkp_FromREPlot()
{
MarkPoint resMkp = new MarkPoint(R1);
// 自己位置更新
// ロータリーエンコーダの移動量差分を加えて、自己位置を更新
resMkp.X += (E1.X - oldE1.X);
resMkp.Y += (E1.Y - oldE1.Y);
resMkp.Theta = E1.Theta; // REの向き
oldE1.Set(E1);
return(resMkp);
}
/// <summary>
/// 位置補正執行
/// </summary>
/// <param name="bRevisionFromGPS">GPSの値をそのまま使う</param>
public string LocalizeRevision( bool bRevisionFromGPS )
{
// 補正ログメッセージ
string revisionMsg = "";
// 補正後座標
MarkPoint rev = new MarkPoint(V1);
revisionMsg += "LocRevision:Issue / 座標補正を実行\n";
revisionMsg += "LocRevision: beforR1 R1.X" + R1.X.ToString("f2") + "/R1.Y " + R1.Y.ToString("f2") + "/R1.Dir " + R1.Theta.ToString("f2") + "\n";
// GPSが有効なら、GPSの値を元にPF補正
if (bEnableGPS)
{
// 補正基準 位置,向き
double RivLocX = G1.X;
double RivLocY = G1.Y;
double RivDir = (bActiveCompass ? C1.Theta : R1.Theta); // コンパスが使えるなら、コンパスの値を使う
if (bActiveCompass)
{
// コンパスを使った
revisionMsg += "LocRevision:useCompass C1.Dir " + C1.Theta.ToString("f2") + "\n";
}
if (bRevisionFromGPS)
{
// GPSの値をそのまま使う
rev.Set(RivLocX, RivLocY, RivDir);
// GPSを使った
revisionMsg += "LocRevision: useGPS G1.X" + G1.X.ToString("f2") + "/G1.Y " + G1.Y.ToString("f2") + "/G1.Dir " + G1.Theta.ToString("f2") + "\n";
}
else
{
// 補正基準位置からパーティクルフィルター補正
rev.Set(RivLocX, RivLocY, RivDir);
// V1ローパスフィルターリセット
ResetLPF_V1(rev);
// 自己位置推定演算 10回
CalcLocalize(V1, true,10);
// PF
revisionMsg += "LocRevision: usePF V1.X" + V1.X.ToString("f2") + "/V1.Y " + V1.Y.ToString("f2") + "/V1.Dir " + V1.Theta.ToString("f2") + "\n";
}
}
else
{
// V1の値をそのまま使う
revisionMsg += "LocRevision: V1toR1 V1.X" + V1.X.ToString("f2") + "/ V1.Y " + V1.Y.ToString("f2") + "/ V1.Dir " + V1.Theta.ToString("f2") + "\n";
}
// 結果を反映
R1.Set(rev);
E1.Set(rev);
oldE1.Set(rev);
return revisionMsg;
}