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

Adding covariance matrix to ndt_pose.

asked 2022-06-30 06:29:07 -0600

Azeem gravatar image

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

temp_list = list()

for i in (state):
    i = i - (sum(state) / len(state))
    i = i * 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.

So could someone please help me out or If I'm doing something wrong could you please point it out. Thanks in advance!.

edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted

answered 2022-06-30 09:06:54 -0600

Yamato Ando gravatar image


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.

edit flag offensive delete link 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?

Azeem gravatar image Azeem  ( 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 :)

Yamato Ando gravatar image Yamato Ando  ( 2022-07-01 04:22:42 -0600 )edit

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

Azeem gravatar image Azeem  ( 2022-07-02 02:47:47 -0600 )edit

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

Yamato Ando gravatar image Yamato Ando  ( 2022-07-05 04:23:07 -0600 )edit

Question Tools

1 follower


Asked: 2022-06-30 06:29:07 -0600

Seen: 101 times

Last updated: Jun 30 '22