Robotics StackExchange | Archived questions

How to use the covariance matrix of the robot_localization package

I am trying to fuse IMU and GPS odometry using the ekfrobotlocalization node. I would like the filtered odometry to be more dependent on the IMU and less on the GPS as the noise of the GPS is quite visible in the filtered output. I figured that adapting the covariance matrix will be able to give me this result by increasing the variation of the GPS measurements. However, i cannon find any information on what each variable in the covariance matrix relates to. Could someone explain the function of each variable in the covariance matrix and how to let the filtered output be more dependent of a specific sensor?

Asked by someDutchguy on 2020-11-24 03:52:33 UTC

Comments

@someDutchguy any follow up on this?

Asked by JackB on 2020-11-28 11:24:40 UTC

Answers

For the simple case most often considered by general users the covariance matrix is diagonal matrix. Each element on the diagonal can be considered as the variance of the corresponding state in the state vector.

The state is a length 15 vector with members given as (X,Y,Z,roll,pitch,yaw,X˙,Y˙,Z˙,roll˙,pitch˙,yaw˙,X¨,Y¨,Z¨). Where single dot is the first time derivative and two dot is the second time derivative (sorry about the messed up format :/).

With that understood, the element (1,1) of the covariance matrix is the variance of X, element (2,2) is the variance of Y, element (3,3) is the variance of Y and so on down the line.

To understand the covariance matrix in general check out the wikipedia page. And note that for the simple case that most users encounter (myself included) we do not consider cross correlation between states. That is why we the covariance matrix is a diagonal matrix.

So in order to make your GPS measurement have less of an impact on the estimate, you should figure out how to increase its (1,1) and (2,2) covariance matrix values because it only should measure (x,y) location and increasing the variance tells the filter to "trust" the measurement less.

Hope this helps! Cheers

Asked by JackB on 2020-11-24 11:24:40 UTC

Comments