Adding covariance matrix to ndt_pose.

edit

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.

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