3

現在、 Free C++ Extended Kalman Filter Libraryを使用しようとしています。カルマン フィルターの基本は理解していますが、このライブラリで NaN 値が生成されるという問題があります。SO の誰かがカルマン フィルター アルゴリズムを使用して私の間違いを見つけた経験がありますか?

これは私のフィルターです：

class PointEKF : public Kalman::EKFilter<double,1,false,true,false> {
public:
        PointEKF() : Period(0.0) {
            setDim(3, 1, 3, 1, 1);
        }

        void SetPeriod(double p) {
            Period = p;
        }
protected:
        void makeBaseA() {
            A(1, 1) = 1.0;
            //A(1, 2) = Period;
            //A(1, 3) = Period*Period / 2;
            A(2, 1) = 0.0;
            A(2, 2) = 1.0;
            //A(2, 3) = Period;
            A(3, 1) = 0.0;
            A(3, 2) = 0.0;
            A(3, 3) = 1.0;
        }
        void makeBaseH() {
            H(1, 1) = 1.0;
            H(1, 2) = 0.0;
            H(1, 3) = 0.0;
        }
        void makeBaseV() { 
            V(1, 1) = 1.0;
        }
        void makeBaseW() {
            W(1, 1) = 1.0;
            W(1, 2) = 0.0;
            W(1, 3) = 0.0;
            W(2, 1) = 0.0;
            W(2, 2) = 1.0;
            W(2, 3) = 0.0;
            W(3, 1) = 0.0;
            W(3, 2) = 0.0;
            W(3, 3) = 1.0;
        }

        void makeA() {
            double T = Period;
            A(1, 1) = 1.0;
            A(1, 2) = T;
            A(1, 3) = (T*T) / 2;
            A(2, 1) = 0.0;
            A(2, 2) = 1.0;
            A(3, 3) = T;
            A(3, 1) = 0.0;
            A(3, 2) = 0.0;
            A(3, 3) = 1.0;
        }
        void makeH() {
            double T = Period;
            H(1, 1) = 1.0;
            H(1, 2) = T;
            H(1, 3) = T*T / 2;
        }
        void makeProcess() {
            double T = u(1);
            Vector x_(x.size());
            x_(1) = x(1) + x(2) * T + (x(3) * T*T / 2);
            x_(2) = x(2) + x(3) * T;
            x_(3) = x(3);
            x.swap(x_);
        }
        void makeMeasure() {
            z(1) = x(1);
        }

        double Period;
};

次のように使用しました。

void init() {
    int n = 3;
    static const double _P0[] = {
                                 1.0, 0.0, 0.0,
                                 0.0, 1.0, 0.0,
                                 0.0, 0.0, 1.0
                                };
    Matrix P0(n, n, _P0);
    Vector x(3);
    x(1) = getPoint(0);
    x(2) = getVelocity(0);
    x(3) = getAccleration(0);
    filterX.init(x, P0);
}

と、

    Vector measurement(1), input(1), u(1);
    u(1) = 0.400;
    double start = data2->positionTimeCounter;
    double end = data->positionTimeCounter;
    double period = (end - start) / (1000*1000);
    filterX.SetPeriod(period);
    measurement(1) = getPoint(0);
    input(1) = period;
    filterX.step(input, measurement);
    auto x = filterX.predict(u);

注: 私が使用しているデータは、単位円から生成された x ポイントです。

4

1 に答える 1

4

マトリックスの基本バージョンを使用する場合:

A = [ 1 0 0;
      0 1 0;
      0 0 1 ];
H = [ 1 0 0 ];

測定値は最初の状態 (位置) のみをキャプチャし、A マトリックスには位置とその導関数 (速度、加速度) の間に結合がないため、観測可能なシステムはありません。可観測性マトリックスは次のとおりです。

O = [ H;
      H*A;
      H*A*A ];
O = [ 1 0 0;
      1 0 0;
      1 0 0 ];

これは明らかに特異です。つまり、システムは観測できません。そして、EKFアルゴリズムを介してそれをフィードするとエラーが発生するはずです（状況はアルゴリズムによって検出される必要があります）が、検出されない場合は、経験しているように、推定でNaNの結果になります。

ここで、makeA() 関数の A 行列の方が適しています。

A = [ 1 h h*h/2;
      0 1 h;
      0 0 1 ];
H = [ 1 0 0 ];       // use this H matrix (not [ 1 h h*h/2 ])

可観測性マトリックスにつながる：

O = [ 1    0      0;
      1    h  h*h/2;
      1  2*h  2*h*h ];

これはフルランク (特異ではない) であるため、観察可能なシステムが得られます。

カルマン フィルター処理アルゴリズムは、行列の条件付けに非常に敏感になる可能性があります。つまり、時間ステップが非常に小さい場合 (例: 1e-6)、連続時間バージョンを使用する必要があります。また、NaN の問題は、KF アルゴリズムで必要な線形ソルバー (線形連立方程式を解く) に起因する可能性があります。ライブラリの作成者が素朴な方法 (たとえば、ガウス消去、ピボットの有無にかかわらず LU 分解、ピボットのないコレスキーなど) を使用した場合、この数値条件付けの問題はさらに悪化します。

NB 初期の P は初期状態ベクトルの不確実性を反映する必要があるため、非常に高い P 行列で KF フィルタリングを開始する必要があります。これは通常非常に高く、P は約1000 * identityです。

于 2013-01-10T16:47:14.273 に答える