Answers.Body: <p>From your matrix definitions, the issue is S always singular so it can never be inverted </p>
  
  <p>I reimplemented the computation with numpy and here are the numbers </p>
  
  
  
  <pre><code>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
  
  </code></pre>
  
  <p>and <code>S</code> is </p>
  
  <pre><code>array([[1100.,    0.,    0.,    0.],
         [   0., 1100.,    0.,    0.],
         [   0.,    0.,    0.,    0.],
         [   0.,    0.,    0.,    0.]])
  
  </code></pre>
  
  <p>Basically the definition of <code>H</code> 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 </p>
  
Answers.ContentLicense: CC BY-SA 4.0
Answers.CreationDate: 2019-04-26T07:21:31.433Z
Answers.LastActivityDate: 2019-04-26T07:21:31.433Z
Answers.UserDisplayName: Nicola Bernini
Answers.Body: <p>Check to see if the determinant of S is zero before you do the inverse. If that is the case, use pseudo inverse.</p>
  
Answers.ContentLicense: CC BY-SA 4.0
Answers.CreationDate: 2018-05-30T15:34:42.707Z
Answers.LastActivityDate: 2018-05-30T20:05:19.580Z
Answers.LastEditDate: 2018-05-30T20:05:19.580Z
Answers.UserDisplayName: Daniel Martinez
Body: <p>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. <a href="https://en.wikipedia.org/wiki/Kalman_filter" rel="nofollow noreferrer">Here</a> 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. </p>
  
  <pre><code>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_;
  </code></pre>
  
Comments.ContentLicense: CC BY-SA 4.0
Comments.CreationDate: 2018-06-01T08:49:05.723Z
Comments.Text: This question should be migrated to: https://datascience.stackexchange.com/.
Comments.UserDisplayName: JahKnows
Comments.ContentLicense: CC BY-SA 4.0
Comments.CreationDate: 2021-05-06T18:03:43.013Z
Comments.Text: I don't any questions here. Can you clarify what your question was?
Comments.UserDisplayName: nbro
Tags: <computer-vision>
Title: Kalman filter pre inovation
UserDisplayName: Mircea Paul Muresan
ThreadText: # Kalman filter pre inovation
  
  (Asked by: username_0 on 2018-05-21)
  
  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_;
  
  **Tags:** computer-vision
  > **Comments:**
  > **username_1**: This question should be migrated to: https://datascience.stackexchange.com/.  
  > **username_2**: I don't any questions here. Can you clarify what your question was?  
  
  ---
  
  ## 2 answers
  
  ### Answer by username_3 on 2019-04-26. Score: 1 | Accepted answer
  
  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
  
  
  ---
  
  ### Answer by username_4 on 2018-05-30. Score: 0
  
  Check to see if the determinant of S is zero before you do the inverse. If that is the case, use pseudo inverse.
  
  
  ---
  
  