Kalman filter pre inovation 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-&gt;lidar_R &lt;&lt; std_laspx_, 0, 0, 0,
    0, std_laspy_, 0, 0,
    0, 0, 0, 0,
    0, 0, 0, 0;

this-&gt;lidar_H &lt;&lt; 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_ &lt;&lt; 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-&gt;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-&gt;x_ = this-&gt;x_ + K*y;
MatrixXd I = MatrixXd::Identity(x_.size(), x_.size());
this-&gt;P_ = (I - K * H) * this-&gt;P_;
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