ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange

# Adding covariance matrix to ndt_pose.

I am trying to fuse the pose that I get from NDT with the pose that I generate from the vehicle's sensors (IMU and wheel encoders) with the help of robot_localization package.

As we know, since ndt_pose topic does not have covariance data (i.e, it is of pose stamped), which is required for filters. I surfed the internet and found this blog wherein the user has mentioned not to use fitness_score value as a means of calculating covariance.

I read this blog, where the user gives a pretty decent explanation of how to compute covariance for a robot.

After reading these, I decided to compute the covariance values manually, by subscribing to ndt_pose topic and then reading x, y and yaw state values to further compute mean and covariance.

After reading this blog, I thought the way to calculate covariance for a state is by using the formula;

Cov (X,X) = Var (X)

{By a mathematical property, linked here}

and the fomula for variance is in this website under sample variance section.

Note: I am calculating sample covariance not the population covariance

I then started calculating covariance by creating a list which comprises of 15 zero's and once I read a data from ndt_pose topic, I'll pop the oldest value in the list (i.e, value in index (0)) and append the value read to the last thereby creating a moving filter effect.

The logic I created in code is as follows:

def covar_math(value, state):

state.pop(0)
state.append(value)
temp_list = list()

for i in (state):
i = i - (sum(state) / len(state))
i = i * i
temp_list.append(i)

cov = sum(temp_list) / (len(temp_list) - 1) #Since I am calculating sample covariance not population
print (cov)

return cov


This code takes in the state value from NDT (be it X , Y or theta) along with the state_name for covariance computation (for my internal understanding)

After this my script generates covariance values but I don't know how to validate or check if the covariance matrix generated is good or not.

edit retag close merge delete

Sort by » oldest newest most voted

Hi.

When calculating the covariance, do you use data while the vehicle is stopped?

Please note that if the vehicle is moving, I think the covariance will not be calculated correctly because the state values keep changing.

more

Okay, but in that case how will I generate dynamic covariance?. Bcoz if I calculate covariance while the vehicle is stationary and only use that won't it be like a static covariance?

That is one way to do it. The latest Autoware also sets a fixed covariance.

I think the covariance should be changed dynamically according to each environment, but it is difficult how to find the appropriate value. If you have a better way, please let me know :)

Okay sure. By the way is the approach that I have currently taken is wrong?

After all the zeros made for initialization have been popped, I think it works well.