人工智能 - 卡尔曼滤波器预创新 - 吾爱随笔录

我正在尝试使用卡尔曼滤波器跟踪 LIDAR 对象。问题是创新的值为 0，这使得卡尔曼增益为无穷大。这是卡尔曼方程的链接。下面列出了我初始化测量和过程协方差矩阵的值。更新代码也如下所示。当我调试代码时，一切都很好，直到创新变为 0。

this->lidar_R << std_laspx_, 0, 0, 0,
    0, std_laspy_, 0, 0,
    0, 0, 0, 0,
    0, 0, 0, 0;

this->lidar_H << 1.0, 0.0, 0.0, 0.0, 0.0,
    0.0, 1.0, 0.0, 0.0, 0.0,
    0.0, 0.0, 0.0, 0.0, 0.0,
    0.0, 0.0, 0.0, 0.0, 0.0;

P_ << 1000, 0, 0, 0, 0,
    0, 1000, 0, 0, 0,
    0, 0, 1000, 0, 0,
    0, 0, 0, 1000, 0,
    0, 0, 0, 0, 1000;

 MatrixXd PHt = this->P_ * H.transpose();
 //S becomes 0
 MatrixXd S = H * PHt + R;
 //S_inv becomes INFINITY
 MatrixXd S_inv_ = S.inverse();
 MatrixXd K = PHt * S_inv_;

VectorXd y = Z - Hx;

this->x_ = this->x_ + K*y;
MatrixXd I = MatrixXd::Identity(x_.size(), x_.size());
this->P_ = (I - K * H) * this->P_;

H = np.array([[1.0, 0.0, 0.0, 0.0, 0.0], [0.0, 1.0, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 0.0, 0.0]]) P = np.identity(5) * 1000 R = np.array([[100, 0, 0, 0], [0, 100, 0, 0], [0,0,0,0], [0,0,0,0]]) S=H.dot(P.dot(np.transpose(H)))+R