I am facing the following issue, and some hint or orientation would be great. I am working on a project in which I was given a ROS bag, and I have to implement a localization node using SLAM. To do so, I have developed the prediction and correction part of the algorithm, and I am now working on adding new landmarks to the map (there are 2 kind of landmarks). My current problem is that, for one of the landmark types, I don’t have the noise covariance matrix for the correction step (sometimes referred as Q, sometimes referred as R). The observations for this kind of landmarks is of type `geometry_msgs/Pose`

.

I want to know if from the ROS bag data, I can estimate the covariance matrix for the observations of that kind of landmark. I’ve tried the following things:

- importing the data to Matlab, and calculate the covariance matrix using the
`cov`

function. The result is a 6×6 covariance matrix, with values that go from`-0.1`

to`8.XX`

. With this, the result in the robot’s position is very noisy. - importing the data to Matlab and calculate the correlation matrix using the corr function. The result using this values is a bit better than the previous attempt but still noisy.
- I tried an Identity matrix filled with some values in the order of
`1e-3`

in the variance fields. I did this assuming that the position of the landmark has no correlation with its orientation, as well as the x position has no correlation with its y position (something that I now think it’s not true). The result in this case is something a bit less noisy than the first point, but more than the second point.

So, here is my question: is there a way I can estimate the measurement’s noise covariance matrix from the ROS bag?

Hope you can help me! Thanks in advance!

submitted by /u/adiego73

[link] [comments]