ジャイロスコープと加速度計データを組み合わせます

Question

LEGO MindstormのNXTシステムを使用して、バランスのとれたロボットを構築しています。私はHitechnicの2つのセンサーを使用していますが、1つ目は加速度計で、2つ目はジャイロスコープです。 -90〜90度の範囲で両方のセンサーと派生角からノイズを正常にフィルタリングし、0度は完全にバランスが取れています。

私の次の課題は、両方のセンサー値を組み合わせて、時間の経過とともにジャイロスコープのドリフトを修正することです。以下は、ジャイロスコープからのドリフトを実証するために実際のデータから作成したサンプルグラフです。

これらのセンサーを堅実に組み合わせるために私が見た中で最も一般的に使用されるアプローチは、Kalmanフィルターを使用することです。しかし、私は計算の専門家ではなく、数学的なシンボルを本当に理解していませんが、ソースコードで数学を理解しています。

私はrobotc（他のC派生語と同じです）を使用していますが、誰かがCでこれを達成する方法の例を私に与えることができれば本当に感謝しています。

ご協力ありがとうございました！

解決策の結果：

さて、Kersnyは補完的なフィルターを紹介することで私の問題を解決しました。これは私の結果を示すグラフです：

結果＃1

結果＃2

ご覧のとおり、フィルターはジャイロスコープドリフトを修正し、両方の信号を単一の滑らかな信号に結合します。

編集： とにかく壊れた画像を修正していたので、このデータを生成するために使用したリグを表示するのは楽しいと思いました。

Answer 1

Kalman Filtersは素晴らしく、すべてですが、補完的なフィルターは、同様の結果で実装がはるかに簡単だと思います。補完的なフィルターをコーディングするために見つけた最高の記事は このウィキ （一緒に この記事 センサーをエンジニアリングユニットに変換することについて）とzipファイルのpdf このページ （技術文書の下で、zipのファイル名はfilter.pdfであると思います）;

詩カルマンフィルターで立ち往生している場合、 ここ ArduinoのC-Syntaxコードは、それを実装しています。

Answer 2

gamasutra.com 走った WiimoteフィルタリングにKalmanフィルターを使用する記事.

記事の最後には、C ++ソースコードへのリンクがいくつかあります。