Covariance values on Piksi ROS Odometry Message
Why are the covariance values for position so high in the Odometry message published by swiftnav_ros package (piksi multi RTK setup is being used, with succesful RTK fix.)
header:
seq: 420
stamp:
secs: 1525036934
nsecs: 533197224
frame_id: "gps"
child_frame_id: ''
pose:
pose:
position:
x: 2.29
y: 3.038
z: -0.516
orientation:
x: 0.0
y: 0.0
z: 0.0
w: 0.0
covariance: [100000000.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 100000000.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 484000000.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1000.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1000.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1000.0]
twist:
twist:
linear:
x: -0.004
y: 0.008
z: 0.008
angular:
x: 0.0
y: 0.0
z: 0.0
covariance: [0.015210000000000001, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.015210000000000001, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.03198, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1000.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1000.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1000.0]
Notice the values in the covariance in the position. I notice that in the driver, the covariance fields are populated as such:
// populate the pose covariance matrix if we have a good fix
double h_covariance = (sbp_ned.h_accuracy * sbp_ned.h_accuracy) / 1.0e-6;
double v_covariance = (sbp_ned.v_accuracy * sbp_ned.v_accuracy) / 1.0e-6;
// Pose x/y/z covariance
rtk_odom_msg->pose.covariance[0] = h_covariance; // x = 0, 0 in the 6x6 cov matrix
rtk_odom_msg->pose.covariance[7] = h_covariance; // y = 1, 1
rtk_odom_msg->pose.covariance[14] = v_covariance; // z = 2, 2
Why is there a division by 1.0e-6?
My guess is that the driver just wants to show the accuracy cannot be trusted, e.g your filter should ignore it. Which driver are you using?
I guess so, I recloned the ros_swiftnav driver and the code actually changed.. now h_covariance = pow(sbp_ned.h_accuracy * 1e-3, 2), and v_covariance = pow(sbp_ned.v_accuracy * 1e-3, 2), which gives much more realistic and reasonable values..