4

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_;
Oliver Mason
  • 5,387
  • 13
  • 32
Mircea Paul Muresan
  • 153
  • 2

2 Answers2

1

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

Nicola Bernini
  • 809
  • 6
  • 12
0

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

Daniel Martinez
  • 1
  • 1