Estimate noise covariance matrix of measurements using a ros-bag

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]