Estimate noise covariance matrix of measurements using a ros-bag

Question

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 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?

---

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!

Answer 1

You can estimate the noise in the kf by adding a covariance state to your state vector. I don't know how practice this is but it can be done.