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?

( 2022-06-30 20:16:10 -0600 )edit

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 :)

( 2022-07-01 04:22:42 -0600 )edit

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

( 2022-07-02 02:47:47 -0600 )edit

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

( 2022-07-05 04:23:07 -0600 )edit