0
$\begingroup$

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!

Improve this question
$\endgroup$
2
  • $\begingroup$ Isn't the whole point of the covariance matrix to model the noise? Or do you mean when you say that the position is noisy that it more noisy than the real measurements? $\endgroup$ – fibonatic Aug 7 '20 at 20:19
  • $\begingroup$ I use the landmarks to update the position of the robot, and so, the covariance matrix that models the noise of the observation is important: it is used to calculate the kalman gain and to enlarge the state's covariance matrix (sigma). I need to estimate the noise covariance matrix of the observations, from the rosbag $\endgroup$ – adiego73 Aug 8 '20 at 11:14
0
$\begingroup$

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.

Improve this answer
$\endgroup$
2

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service, privacy policy and cookie policy

Not the answer you're looking for? Browse other questions tagged or ask your own question.