方向検出用のカルマン フィルターを実装しようとしています。オンラインで見つけた他のほとんどの実装と同様に、ジャイロと加速度計を使用してピッチとロールを測定しますが、水平検出も追加する予定です。これにより、ピッチとロールの 2 番目の読み取り値が得られます。これは、ジャイロを制御に使用しながら、現在の状態を測定する 2 つの手段、加速度計と水平線検出を使用することを意味します。
これまでのところ、このチュートリアルに基づいて、センサー データと地平線検出にフィルターを個別に実装しました。実装する/
アルゴリズムが予測された状態、加速度計の読み取り値、および水平線で検出された読み取り値の間で最適な読み取り値を選択するには、カルマン フィルターのどの部分を変更する必要がありますか? ヘルプ、論文またはサイトへのリンクは大歓迎です。
KF は 2 つの並列コンポーネントで構成されます: 1. 推定された状態、および 2. その推定の不確実性 (具体的には、状態コンポーネントの共分散行列)。
状態の 2 つの推定値を組み合わせる場合、標準的な方法では加重平均が取られ、加重は (共) 分散の逆数になります。つまり、2 つの推定値のうち確実性が高い (共分散が小さい) 方が、他の推定値よりも高く重み付けされます。
したがって、2 つの推定値の共分散をまだ追跡していない場合は、これを行う必要があります。
2 つの推定値 X' と X" を持つスカラー状態 X の場合、各推定値の分散は V' と V" になり、逆行列 C' = 1/V' と C" = 1/V" になります。(「確実性」C は、分散 V よりも使いやすいです。)
次に、状態の MMSE 推定値 (KF が最適化しようとするもの) は、Xmmse = (X'/V' + X"/V") / (1/V' + 1/V") で与えられます。[この時点で、V' と V" に基づいて、対応する V への更新もあります。]
ベクトル状態の場合、V は共分散行列に置き換えられ、除算は逆行列になります。この場合、逆 C を直接追跡する方が簡単かもしれません: Xmmse = (C' + C") \ (C' * X' + C" * X") [および対応する C への更新]。は、最初の因数 C'+C" の逆数による事前乗算を示します。
これが役立つことを願っています。
[ここで書式設定が不十分であることをお詫びします。スタック オーバーフローは、代数式がコードであると推測し、回答する前にそれらをコードとしてフォーマットすることを要求しました。そうではないので、できませんでした。]