# 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->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: datascience.stackexchange.com. Jun 1 '18 at 8:49
• I don't any questions here. Can you clarify what your question was?
– nbro
May 6 at 18:03

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

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