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 6x6 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 thex
position has no correlation with itsy
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?
Edit:
To clarify a bit what I need: The EKF is composed by a prediction step and a correction step. The prediction step is:
$\hat\mu = g(\mu_{t})$
$\hat\Sigma = G_t \hat\Sigma G_t^T + Q_t$
The correction step is
$K_t = \hat\Sigma H_t^T ( H_t \hat\Sigma H_t^T + R_t )^{-1}$
$\mu_{t+1} = \hat\mu + K_t ( z_t - h ( \hat\mu ) )$
$\Sigma_{t+1} = ( I - K_t H_t ) \hat\Sigma$
I am trying to estimate $R_t$
Hope you can help me! Thanks in advance!