カルマン、最小の正方形、または

Question

私が質問していることを人々が理解するのを助ける努力として、私はそれを完全に言い直すことにしました。私はこれがそれをクリアすることを願っています。

私は1秒のレートでGPSデータ（緯度/経度）を収集しています。このデータは100％正確ではなく、時には1つ以上のデータポイントがありますが、かなり正確なコースを決定するためにアウトライアポイントを削除する最も適切な方法は何でしょうか。車両の速度？この車両は、通常0〜60マイル/時のどこからでも、一般的に直線で移動できますが、急な転回（加重値ですか？）も起こりえます。

私は混乱していることをお詫び申し上げますが、すでに手渡されている提案を理解できていないためです。

Answer 1

標準アルゴリズムadjacent_differenceは、イテレータの範囲内の各要素間に差異をもたらします。だから5つの要素がある場合、それは4つの違いを生み出します。私はあなたのGPSクラスがどのようになるか分からない

#include <vector> 
#include <iostream> 
#include <iterator> 
#include <algorithm> 
#include <numeric> 

：

これらは、私たちが使うことになる標準ライブラリです。私はそれは次元の一つだと仮定するつもりです：

class Position 
{ 
public: 
    Position() : 
    m_position(0) 
    { 
    } 

    Position(int position) : 
    m_position(position) 
    { 
    } 

    Position operator-(const Position& other) const 
    { 
     return Position(m_position - other.m_position); 
    } 
    operator int() const 
    { 
     return m_position; 
    } 
private: 
    int m_position; 
}; 

Position abs_sum(const Position& lhs, const Position& rhs) 
{ 
    return Position(abs(int(lhs)) + abs(int(rhs))); 
} 

はそれを一緒に置く：

int main() 
{ 
    using namespace std; // for brevity - don't really do this in your code 

    vector<Position> positions; 
    positions.push_back(Position(13)); 
    positions.push_back(Position(23)); 
    positions.push_back(Position(17)); 
    positions.push_back(Position(19)); 

    vector<Position> displacements; 

    adjacent_difference(positions.begin(), positions.end(), 
         back_inserter(displacements)); 

    cout << "Displacements: "; 
    copy(displacements.begin(), displacements.end(), 
     ostream_iterator<int>(cout, ", ")); 

    cout << endl; 

    int distance = accumulate(displacements.begin(), displacements.end(), 
           0, abs_sum); 
    cout << "Total: " << distance << endl; 

    return 0; 
} 

出力：あなたがいないので

Displacements: 13, 10, -6, 2, 
Total: 31 

Answer 2

問題は病気に形成されたように見えます十分なデータがある。だから、あなたのGPSは、位置を収集しています、これらは基本的に座標の束です。そして、あなたは「彼らが正しい」と「それをもっと正確にする」ことを求めています。明らかに、これを行うにはさらに多くのデータが必要です。

ロボットの場合、典型的な「別のデータ」は、他のセンサー（IMU - 慣性計測ユニット、実質的に加速度計など）やオドメトリー（モーターへのコマンド）などのデータです。これらの両方から、ロボットはそれが「まっすぐ」であることを知っており、左右にある任意のオフシューズを修正することができる。あるいは、コンピュータ視覚アルゴリズムを使用して「ランドマーク」（木やコーナーなど）を追跡し、ロボットの動きに関する良い情報も提供します。あなたはそれらのどれも持っていません。

あなたが持っているものは、車の物理モデルです。あなたは60 MPHで、車は90度回転することはできないことを知っています（なぜ車がどのように動くべきかをあなたが自然に知っているので、問題はあなたにはあまり適していないようです）。この制約は、センサ情報が追加されるほど良好ではありませんが、そうする必要があります。非線形最小2乗またはカルマンフィルタを同様に使用できます。

私はカルマンフィルタの大ファンではありませんので、その実装方法は教えません。

NLSを使用すると、車の位置がグラフとして表示されます（プロットと混同しないでください）。車の各位置は頂点です。隣接する2つの頂点（「前の位置と現在の位置」に対応）はすべて、「車の動きの法則」の制約（エッジ）によってリンクされています。各頂点にはGPS位置制約もあり、これは単項エッジです。

この種のグラフは、疎なマトリックスで表されます。これはヤコビ行列（またはヘッセ行列）であり、値は与えられたシステム状態（すべての頂点の位置）での制約に関する頂点の導出に対応する。各ステップで、次の位置が追加されるため、マトリックスサイズが大きくなります。ソリューションの複雑さを低く抑えるために、最後のNステップだけを保持して古いポジションを削除することができます。各ステップで、物理的制約（回転速度と回転速度を計算し、それが妥当かどうかを調べる）とヤコビアン/ヘッセ行列と誤差ベクトル（GPS修正からの現在のベクトル位置の差のベクトル/物理的に拘束されたものモーション）。次に、このシステム（dx = Jacobians/errors）を解くと、頂点の位置の違いであるベクトルdxが得られます。頂点に追加するだけで、そこにいます。それは本質的にGauss-Newtonアルゴリズムです。

これは簡単に実装できません。 SLAM++,iSAM,​​またはg2oのような、この種のグラフ問題を効率的に解決するライブラリがあります。問題は、物理的な妥当性制約がそこに実装されていない（GPS制約もないが、それは単なる減算である）ので、これらのどれもすぐに使えることではない。独自の頂点/エッジタイプを実装する必要があります。

もっと簡単な方法をお勧めします。 GPSがあなたに伝えていることの違いを取って、ウィンドウ中央値を計算し、最後の測定値が中央値から離れすぎているかどうかを確認してください。それがあまりにも遠すぎる場合（あなたは実験をしてどの閾値が働くかを見なければならないでしょう）、スピード/コースの計算にその測定値を使用しないでください（しかし、中央値の計算にはそれを保ちます）。それはかなり正確で、あなたの目的のために大丈夫でしょう。

測定データを、あなたがブロックを走らせている緯度/経度/タイムスタンプのテキストファイルとしてアップロードすると、そのコードを処理することができます。