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_;
```

This question should be migrated to: https://datascience.stackexchange.com/.

– JahKnows – 2018-06-01T08:49:05.723