3

3 軸加速度計と 3 軸ジャイロスコープをセンサーとして使用して、カルマン フィルターを実装してオブジェクトの向きを取得しようとしています。

このフィルターの予測フェーズの動的モデルの選択は簡単です。

new_angle = angle + angular_velocity * time
new_angular_velocity = angular_velocity

しかし、私は手元に浮動小数点のサポートがなく、角度をモデル化するためにあらゆる精度が必要です。したがって、私の計画では、角度を 32 ビットの整数データとして表し、2 pi2^32 の小さなステップとして 1 回転を表すことにしました。結果として、整数オーバーフローはラップ (2 piと同じ向き0) を無料で処理します。

しかし、これはフィルターにも問題をもたらします。たとえば、推定角度が で、359°私の測定値がである場合、フィルターは大きな革新を想定しており、不確実性と奇数値につながります。

この可能なラップをフィルターに認識させる方法はありますか? 上記の場合のみの革新を与える

この問題を回避するために、角度の代わりに角度差を使用することを考えましたが、適切なモデルが見つかりません。

4

1 に答える 1

2

私は今日、同様の KF で同じ問題を抱えていました...しかし、私はあなたほど拘束されていませんでした。

これが私がしたことです:

  1. predict_angle 値が計算された直後:

    // 0 から 360 の間のジャンプを
    避ける
    if (測定角度 > 3*pi/2 && 推定角度 < pi/2) 予測角度 = 予測角度 + 2*pi;

  2. 計算後、推定角度を正規化します。

少し精度を犠牲にしても構わない場合は、符号付き整数 [-2*Pi,+2*Pi) の範囲に切り替えて、同じことを行ってください。

PS サンプルごとの最大角度変化が小さい場合、KF の内部角度オフセットを使用して、1 ビット精度のほんの一部を犠牲にすることができると思います。オフセットは、その値よりも十分大きくなければなりません。2*pi ではなく、2^32 = 2*Pi + 2*OFFSET の範囲になります。KF で、この OFFSET をローカル角度変数に追加し、estimated_angle = normalize(offset_estimated_angle - OFFSET, 0-2*pi) を返します。

HTH

于 2014-08-28T19:48:38.610 に答える