複数の位置推定の融合

ロボットの位置を推定するために2つのサブシステムがあるシステムがあります。最初のサブシステムは、ロボットが保持しているマーカーを検出するために使用され、ロボットの位置と向きの3つの推定値を出力する3つのカメラで構成されています。2番目のサブシステムは、ロボットに配置され、ロボットの2点の速度を測定するシステムです。これら2つを数値的に統合することで、ロボットの位置と向きの推定値を取得できます（2つの点を同時に追跡しているため）。

最初のシステムは精度が低くなりますが、2番目のシステムはドリフトします。最初のシステムは約1秒に1回の出力を提供しますが、2番目のシステムはより頻繁に出力します（1秒あたり100〜200回）。

最初のシステムの推定値で位置をリセットするだけではなく（100％正確ではないため）、2番目のセンサーシステムからの累積位置を使用し、それを最初のシステム。また、最初のシステムの3つの推定値をどのように融合するかという質問がありますか？2つの推定値がまったく同じで、3番目の推定値が完全に異なる（たぶんもっと間違っているという意味）ので、純粋な平均よりも良い方法があるはずです。

そのようなシステムでの使用を推奨する融合アルゴリズムはありますか？カルマンフィルターについては知っていますが、2つのシステムが異なる周波数でデータを出力するため、フィルターの使用方法を理解するのに苦労しています。

質問が十分に明確であることを願っています。見積もりをより正確で正確な見積もりに融合するための最良のアプローチは何ですか？

ありがとう

あなたが説明しているのは、基本的にカルマンフィルターを使用するための教科書のケースです。まず、予測ステップが必要です。ロボットのポーズを予測しているとしましょう$(x,y,\theta)$、以前の姿勢推定と高周波速度測定を前提 $(v,\omega)$、 どこ $v$ は線速度であり、 $\omega$ 角速度です。

$P$ ロボットポーズの不確実性を表す3x3共分散行列です。 $Q$ 入力の共分散です（つまり、これらの速度測定はどの程度ノイズが多いですか？） $F$ は、状態に関するモーションモデルのヤコビアンであり、 $G$ 入力に関するヤコビアンです。つまり、

これで、頻繁に修正の更新が行われ、実際に完全な状態が測定されるため、これは非常に単純になります。つまり、

どこ $z_k$ （カメラからの）測定値であり、 $R$その測定に関連する共分散行列です（おそらく対角行列）。この測定値は、予測された測定値（あなたの場合は、最新の姿勢推定値）と比較されます。この単純なケースでは、カルマンゲインは、姿勢の共分散と測定の共分散の合計と比較した、現在の姿勢の共分散の割合です。

さまざまなレートに関する質問に答えるために、予測更新が到着するまで繰り返しモーション更新を実行できます。たとえば、修正を実行する前にモーション更新が100回発生する場合があります。

また、3台のカメラの取り扱い方法についても尋ねました。最も簡単な方法は、それらを順番に処理することです。続けて3つの修正を適用するだけです。もう1つの方法は、それらをスタックして単一の更新を実行することです。このようにするには、修正の更新ステップを調整する必要があります。