C++卡尔曼滤波库生成1.#R(NaN)结果

Question

C++卡尔曼滤波库生成1.#R(NaN)结果

3

我目前在尝试使用免费的C++扩展卡尔曼滤波库。我了解卡尔曼滤波的基础知识，但是我在使用该库时出现了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个点。

- andre

1个回答

网页内容由stack overflow 提供, 点击上面的

可以查看英文原文，
原文链接

- Mikael Persson · Accepted Answer

如果您使用矩阵的基本版本：

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

这意味着系统满秩（非奇异），因此您拥有可观测系统。

Kalman滤波算法对矩阵的条件数很敏感，这意味着如果时间步长非常小（例如1e-6），则需要使用连续时间版本。另外，NaN问题可能来自KF算法中需要的线性求解器（求解线性方程组）。如果库的作者使用了一种朴素的方法（例如高斯消元，带或不带枢轴的LU分解，没有枢轴的Cholesky等），那么这个数值调节的问题会变得更加严重。

注意：应该从非常高的P矩阵开始进行KF滤波，因为初始P应反映对初始状态向量的不确定性，而这通常非常高，因此P应在1000 * identity左右。