5

ジャイロ、加速度計、磁力計でコンパスを作ろうとしています。

acc値と磁力計の値を融合して（回転行列を使用して）方向を取得していますが、かなりうまく機能しています。

しかし今、私は磁気センサーが正確でないときに補償するのを助けるためにジャイロスコープを追加したいと思います。したがって、カルマンフィルターを使用して、2つの結果を融合し、適切にフィルター処理された結果を取得したいと思います（accとmagはすでにlpfを使用してフィルター処理されています）。

私の行列は次のとおりです。

 state(Xk) => {Compass Heading, Rate from the gyro in that axis}.
 transition(Fk) => {{1,dt},{0,1}}
 measurement(Zk) => {Compass Heading, Rate from the gyro in that axis}
 Hk => {{1,0},{0,1}}
 Qk = > {0,0},{0,0}
 Rk => {e^2(compass),0},{0,e^2(gyro)}

そして、これは私のカルマンフィルターの実装です：

public class KalmanFilter {

private Matrix x,F,Q,P,H,K,R;
private Matrix y,s;

public KalmanFilter(){
}

public void setInitialState(Matrix _x, Matrix _p){
    this.x = _x;
    this.P = _p;
}

public void update(Matrix z){
    try {
        y = MatrixMath.subtract(z, MatrixMath.multiply(H, x));
        s = MatrixMath.add(MatrixMath.multiply(MatrixMath.multiply(H, P), 
                        MatrixMath.transpose(H)), R);
        K = MatrixMath.multiply(MatrixMath.multiply(P, H), MatrixMath.inverse(s));
        x = MatrixMath.add(x, MatrixMath.multiply(K, y));
        P = MatrixMath.subtract(P, 
                        MatrixMath.multiply(MatrixMath.multiply(K, H), P));
    } catch (IllegalDimensionException e) {
        e.printStackTrace();
    } catch (NoSquareException e) {
        e.printStackTrace();
    }
    predict();
}

private void predict(){
    try {
        x = MatrixMath.multiply(F, x);
        P = MatrixMath.add(Q, MatrixMath.multiply(MatrixMath.multiply(F, P), 
                        MatrixMath.transpose(F)));
    } catch (IllegalDimensionException e) {
        e.printStackTrace();
    }
}

public Matrix getStateMatirx(){
    return x;
}

public Matrix getCovarianceMatrix(){
    return P;
}

public void setMeasurementMatrix(Matrix h){
    this.H = h;
}

public void setProcessNoiseMatrix(Matrix q){
    this.Q = q;
}

public void setMeasurementNoiseMatrix(Matrix r){
    this.R = r;
}

public void setTransformationMatrix(Matrix f){
    this.F = f;
}
}

最初に、この開始値が与えられます：

 Xk => {0,0}
 Pk => {1000,0},{0,1000}

次に、2つの結果（カルマン1つとコンパス1つ）を監視します。カルマンのものは0から始まり、測定されたもの（コンパス）に関係なく一定の割合で増加し、増加し続けるだけで止まりません...

何が間違っているのか理解できませんか？

4

1 に答える 1

6

あなたが見ている問題は、ジャイロのノイズは非常に低いですが、ゼロ平均ではないということです. あなたの用語を使用するときe^2(gyro)、あなたはそれを主張しているフィルターを実装していますz_gyro = true_gyro + v。v ~ N(0, e^2) 真実はv ~ N(bias, e^2)、バイアスにもいくつかの用語がある場所に似ています(主に静的ターンオンバイアスと温度ドリフトによって引き起こされるバイアスシフト)。その結果、バイアスを統合し、常に回転しています。

そのバイアスを調整する場合 (静止時のジャイロの出力を測定するだけ)、update(imu - bias)代わりに を呼び出すことができますupdate(imu)。バイアスのシフトを考慮して増加させる必要があるかもしれませんがe^2(gyro)、すべてを考慮しようとした場合ほどではありません (補償されていないオフセットはR、磁力計とジャイロの項に比例して固定された方向変位になります)。

最良の方法は、バイアスを状態ベクトルに追加することです。のような結果が得られHk = {{1,0,0},{0,1,1}}ます。これは、予測されたジャイロ測定値が真のレートにバイアス項を加えたものであることを意味します。ここでのカルマン フィルターの魔法は、測定値が単に 2 つの項の和であると言ったとしても、それらはいくつかの重要な点で異なるということです。

  • 船首方位はF実際の回転率 ( による) に関連しているため、を更新するたびに、船首方位と回転率に関連する非対角項がdt状態共分散によって展開されます。PP
  • 同様に、Hバイアスとジャイロレートの関係を説明しました。これは、「回転が速くなるか、バイアスが大きくなる」という考えを表現しているため、フィルターは状態を更新して、ノイズ共分散に基づいてこれら 2 つの可能性のバランスをとります。
  • ではQ、回転速度のプロセス ノイズをかなり高く設定して、測定している予期しない動きを考慮に入れる必要があります。しかし、Qバイアスは非常に急速に進化していないため、for バイアスははるかに小さくなっています (実際、最良のモデルはおそらく 1 次ガウス・マルコフ過程であり、ここでは説明しません。 「制限付きメモリ フィルタ」）。制限では、バイアスの項が 0 (ランダム定数Qとしてのモデリング バイアス) であると想像できますが、これは EKF では数値的にうまく機能せず、バイアス ドリフトのために厳密には正しくありません。
  • 同様に、P_0システムの初期値は、完全に未知の方向/角速度よりも、バイアス項 (データシートに記載されている可能性のある範囲の合計) の方がはるかに小さくなります。
  • 多軸システムでは、偏りは常に軸に伴います (これはハードウェアのプロパティであり、向きとは関係ありません) が、ストラップ ダウンにより、「向き」のような状態に対するジャイロの影響が回転します。 IMU。

EKF がジャイロ バイアスのような値を「学習」するのを見ることは、状態の残りの部分の予測よりもさらに魔法のようです。

于 2014-11-16T21:27:01.417 に答える