Skip to content

Commit

Permalink
Fill out error measurements in GPSFix messages (ROS1) (#72)
Browse files Browse the repository at this point in the history 
Although the comments claimed that error measurements in GPSFix
messages were being filled in from BESTPOS logs, that was not actually
the case.

Fixes #35 for real.

Distribution Statement A; OPSEC #2893

Signed-off-by: P. J. Reed <preed@swri.org>
  • Loading branch information
@pjreed
pjreed authored Nov 20, 2019
1 parent b845016 commit 8fbdbbb
Show file tree
Hide file tree
Showing 2 changed files with 25 additions and 27 deletions.
2 changes: 1 addition & 1 deletion novatel_gps_driver/include/novatel_gps_driver/novatel_gps.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -509,7 +509,7 @@ namespace novatel_gps_driver
boost::circular_buffer<novatel_gps_msgs::NovatelXYZPtr> novatel_xyz_positions_;
boost::circular_buffer<novatel_gps_msgs::NovatelUtmPositionPtr> novatel_utm_positions_;
boost::circular_buffer<novatel_gps_msgs::NovatelVelocityPtr> novatel_velocities_;
boost::circular_buffer<novatel_gps_msgs::NovatelPositionPtr> position_sync_buffer_;
boost::circular_buffer<novatel_gps_msgs::NovatelPositionPtr> bestpos_sync_buffer_;
boost::circular_buffer<novatel_gps_msgs::NovatelHeading2Ptr> heading2_msgs_;
boost::circular_buffer<novatel_gps_msgs::NovatelDualAntennaHeadingPtr> dual_antenna_heading_msgs_;
boost::circular_buffer<novatel_gps_msgs::RangePtr> range_msgs_;
Expand Down
50 changes: 24 additions & 26 deletions novatel_gps_driver/src/novatel_gps.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -71,7 +71,7 @@ namespace novatel_gps_driver
novatel_xyz_positions_(MAX_BUFFER_SIZE),
novatel_utm_positions_(MAX_BUFFER_SIZE),
novatel_velocities_(MAX_BUFFER_SIZE),
position_sync_buffer_(SYNC_BUFFER_SIZE),
bestpos_sync_buffer_(SYNC_BUFFER_SIZE),
heading2_msgs_(MAX_BUFFER_SIZE),
dual_antenna_heading_msgs_(MAX_BUFFER_SIZE),
range_msgs_(MAX_BUFFER_SIZE),
Expand Down Expand Up @@ -360,15 +360,14 @@ namespace novatel_gps_driver
else // The gpgga and gprmc messages are synced
{
bool has_position = false;
bool pop_position = false;

// Iterate over the position message buffer until we find one
// Iterate over the bestpos message buffer until we find one
// that is synced with the gpgga message
while (!position_sync_buffer_.empty())
while (!bestpos_sync_buffer_.empty())
{
// Calculate UTC position time from GPS seconds by subtracting out
// full days and applying the UTC offset
double gps_seconds = position_sync_buffer_.front()->novatel_msg_header.gps_seconds + utc_offset_;
double gps_seconds = bestpos_sync_buffer_.front()->novatel_msg_header.gps_seconds + utc_offset_;
if (gps_seconds < 0)
{
// Handle times around week boundaries
Expand All @@ -388,25 +387,19 @@ namespace novatel_gps_driver
{
pos_dt += 86400.0;
}

if (pos_dt > gpgga_position_sync_tol_)
{
// The position message is more than tol older than the gpgga message,
// discard it and continue
ROS_DEBUG("Discarding a position message that is too old (%f < %f)", position_time, gpgga_time);
position_sync_buffer_.pop_front();
}
else if (-pos_dt > gpgga_position_sync_tol_)
{
// The position message is more than tol ahead of the gpgga message,
// use it but don't pop it
ROS_DEBUG("Waiting because the most recent GPGGA message is too old (%f > %f)", position_time, gpgga_time);
has_position = true;
break;
bestpos_sync_buffer_.pop_front();
}
else //the gpgga and position tol messages are in sync
else
{
// There's a bestpos message that is close enough to the GPGGA message to use it
// It's ok if it's far ahead (even though that's weird), it's better than nothing
has_position = true;
pop_position = true;
break;
}
}
Expand All @@ -429,24 +422,29 @@ namespace novatel_gps_driver

if (has_position)
{
auto& position = bestpos_sync_buffer_.front();
// We have a position message, so we can calculate variances
// from the standard deviations
double sigma_x = position_sync_buffer_.front()->lon_sigma;
double sigma_x = position->lon_sigma;
gps_fix->position_covariance[0] = sigma_x * sigma_x;

double sigma_y = position_sync_buffer_.front()->lat_sigma;
double sigma_y = position->lat_sigma;
gps_fix->position_covariance[4] = sigma_y * sigma_y;

double sigma_z = position_sync_buffer_.front()->height_sigma;
double sigma_z = position->height_sigma;
gps_fix->position_covariance[8] = sigma_z * sigma_z;

// 2D and 3D error values are the DRMS and and MRSE as described in
// https://www.novatel.com/assets/Documents/Bulletins/apn029.pdf
double horz_sum = std::pow(position->lat_sigma, 2) + std::pow(position->lon_sigma, 2);
gps_fix->err_horz = std::sqrt(horz_sum);

gps_fix->err = std::sqrt(horz_sum + std::pow(position->height_sigma, 2));

gps_fix->err_vert = position->height_sigma;

gps_fix->position_covariance_type =
gps_common::GPSFix::COVARIANCE_TYPE_DIAGONAL_KNOWN;

if (pop_position)
{
position_sync_buffer_.pop_front();
}
}

// Add the message to the fix message list
Expand Down Expand Up @@ -1051,7 +1049,7 @@ namespace novatel_gps_driver
novatel_gps_msgs::NovatelPositionPtr position = bestpos_parser_.ParseBinary(msg);
position->header.stamp = stamp;
novatel_positions_.push_back(position);
position_sync_buffer_.push_back(position);
bestpos_sync_buffer_.push_back(position);
break;
}
case BestxyzParser::MESSAGE_ID:
Expand Down Expand Up @@ -1258,7 +1256,7 @@ namespace novatel_gps_driver
novatel_gps_msgs::NovatelPositionPtr position = bestpos_parser_.ParseAscii(sentence);
position->header.stamp = stamp;
novatel_positions_.push_back(position);
position_sync_buffer_.push_back(position);
bestpos_sync_buffer_.push_back(position);
}
else if (sentence.id == "BESTXYZA")
{
Expand Down

2 comments on commit 8fbdbbb

@bill-freeman
Copy link

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I would multiply your horizontal error (err_horz) by at least 2 to get it into the 95% of probability, multiply it by 3 and it goes to 99%

@pjreed
Copy link
Contributor Author

@pjreed pjreed commented on 8fbdbbb Nov 21, 2019

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Good point, I actually just noticed the GPSFix message documentation specifies 95% probability there. I'll make that change in #74 since I'm already rewriting a large chunk of that function.

Please sign in to comment.