Use GPS covariances

[RobertJN64]

Here is the old code that apparently worked?

    Matrix3_3 gps_pos_covar;
    Matrix3_3 gps_vel_covar;
    GPS::get_pos_cov(gps_pos_covar);
    GPS::get_vel_cov(gps_vel_covar);
    gps_pos_covar = gps_pos_covar * 15;
    gps_vel_covar = gps_vel_covar * 5;

    Matrix6_6 R = Matrix6_6::Zero();
    R.block<3, 3>(0, 0) = gps_pos_covar;
    R.block<3, 3>(3, 3) = gps_vel_covar;