Revision history [back]

Looks like it's blowing up trying to invert a matrix. If I had to guess, your measurement covariances are wrong and are making the innovation covariance singular (i.e., uninvertible). You have two main issues that I can see after a quick review:

//Setting the diagonal element of covariance matrix
for(iIndex=0;iIndex <36;iIndex++)
{
    odom.pose.covariance[iIndex]=0.001;
}

In this code block, you're not setting the diagonal indices. You're setting all the indices. The inverse of that matrix alone is singular. What you want is this:

//Setting the diagonal element of covariance matrix
for(iIndex=0;iIndex <36;iIndex+=7)
{
    odom.pose.covariance[iIndex]=0.001;
}

Second, you've zeroed out the covariance matrix for your IMU measurement. No sensor can be completely without error, which is what a zero covariance implies.

for(iIndex=0;iIndex <9;iIndex++)
{
  yaw_sens.orientation_covariance[iIndex]=0;
}

Even though this might not cause the inversion issue you originally posted, you should change it to

for(iIndex=0;iIndex <9;iIndex+=4)
{
  yaw_sens.orientation_covariance[iIndex]=0.1; // Or whatever you want. Just make it non-zero and positive.
}

I'm pretty sure the covariances arrays in messages default to all zeros anyway, so you shouldn't have to handle the off-diagonal elements.

Looks like it's blowing up trying to invert a matrix. If I had to guess, your measurement covariances are wrong and are making the innovation covariance singular (i.e., uninvertible). You have two main issues that I can see after a quick review:

//Setting the diagonal element of covariance matrix
for(iIndex=0;iIndex <36;iIndex++)
{
    odom.pose.covariance[iIndex]=0.001;
}

In this code block, you're not setting the diagonal indices. You're setting all the indices. The inverse of that matrix alone is singular. What you want is this:

//Setting the diagonal element of covariance matrix
for(iIndex=0;iIndex <36;iIndex+=7)
{
    odom.pose.covariance[iIndex]=0.001;
}

Second, you've zeroed out the covariance matrix for your IMU measurement. No sensor can be completely without error, which is what a zero covariance implies.

for(iIndex=0;iIndex <9;iIndex++)
{
  yaw_sens.orientation_covariance[iIndex]=0;
}

Even though this might not cause the inversion issue you originally posted, you should change it to

for(iIndex=0;iIndex <9;iIndex+=4)
{
  yaw_sens.orientation_covariance[iIndex]=0.1; // Or whatever you want. Just make it non-zero and positive.
}

I'm pretty sure the covariances covariance arrays in messages default to all zeros anyway, so you shouldn't have to handle the off-diagonal elements.

Looks like it's blowing up trying to invert a matrix. If I had to guess, your measurement covariances are wrong and are making the innovation covariance singular (i.e., uninvertible). You have two main issues that I can see after a quick review:

//Setting the diagonal element of covariance matrix
for(iIndex=0;iIndex <36;iIndex++)
{
    odom.pose.covariance[iIndex]=0.001;
}

In this code block, you're not setting the diagonal indices. You're setting all the indices. The inverse of that matrix alone is singular. What you want is this:

//Setting the diagonal element of covariance matrix
for(iIndex=0;iIndex <36;iIndex+=7)
<36;iIndex+=7) // Note the index increment is now 7
{
    odom.pose.covariance[iIndex]=0.001;
}

Second, you've zeroed out the covariance matrix for your IMU measurement. No sensor can be completely without error, which is what a zero covariance implies.

for(iIndex=0;iIndex <9;iIndex++)
{
  yaw_sens.orientation_covariance[iIndex]=0;
}

Even though this might not cause the inversion issue you originally posted, you should change it to

for(iIndex=0;iIndex <9;iIndex+=4)
<9;iIndex+=4) // Note the index increment is now 4
{
  yaw_sens.orientation_covariance[iIndex]=0.1; // Or whatever you want. Just make it non-zero and positive.
}

I'm pretty sure the covariance arrays in messages default to all zeros anyway, so you shouldn't have to handle the off-diagonal elements.