C++ Kalman filter library producing 1.#R(NaN) results

Question

I'm currently trying to use the Free C++ Extended Kalman Filter Library . I understands the basics of a Kalman filter however I'm having an issue of NaN values being produced with this library. Does anyone on SO have experience using the kalman filter algorithm to spot my mistake?

This is my filter:

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;
};

I used it as follows:

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);
}

and,

    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);

Note: The data I'm using are x points generated from a unit circle.

Answer 1

If you use the Base versions of the matrices:

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

you don't have an observable system because your measurements only capture the first state (position) and there is no coupling, in the A matrix, between position and its derivatives (velocity, acceleration). The observability matrix is as follows:

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

which is obviously singular, i.e., your system is not observable. And feeding that through a EKF algorithm should produce an error (the situation should be detected by the algorithm), but if it is not detected, it will lead to NaN results in the estimates, exactly as you are experiencing.

Now, the A matrix from the makeA() function is more suitable:

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 ])

leading to an observability matrix:

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

which is full-rank (not singular), and thus, you have an observable system.

Kalman filtering algorithm can be quite sensitive to the conditioning of the matrices, meaning that if the time-step is really small (e.g. 1e-6), you need to use a continuous-time version. Also, the problem of NaN might come from the linear solver (solves a linear system of equation) which is needed in the KF algorithm. If the author of the library used a naive method (e.g., Gaussian elimination, LU-decomposition with or without pivots, Cholesky without pivots, etc.), then that would make this issue of numerical conditioning much worse.

N.B. You should start your KF filtering with a very high P matrix, because the initial P should reflect the uncertainty on your initial state vector, which is usually very high, so P should be around 1000 * identity.