4
$\begingroup$

I am trying to track LIDAR objects using Kalman filter. The problem is that the innovation has the value 0, which makes the Kalman gain be Infinity. Here is a link with the Kalman equations. The values with which I initialized the measurement and process covariance matrix are listed below. The update code is also shown below. When I debug the code everything is fine until the innovation becomes 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_;
$\endgroup$
1
$\begingroup$

From your matrix definitions, the issue is S always singular so it can never be inverted

I reimplemented the computation with numpy and here are the numbers

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

and S is

array([[1100.,    0.,    0.,    0.],
       [   0., 1100.,    0.,    0.],
       [   0.,    0.,    0.,    0.],
       [   0.,    0.,    0.,    0.]])

Basically the definition of H with its all zeros lines cancels part of the information (make it unobservable) and you also do not have any observation noise component for them

$\endgroup$
0
$\begingroup$

Check to see if the determinant of S is zero before you do the inverse. If that is the case, use pseudo inverse.

$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service, privacy policy and cookie policy

Not the answer you're looking for? Browse other questions tagged or ask your own question.