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
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
covfunction. The result is a 6×6 covariance matrix, with values that go from
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-3in 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!