Add miss fix detection

eagleye_core/navigation/CMakeLists.txt

@@ -44,7 +44,9 @@ ament_auto_add_library(eagleye_navigation SHARED
   src/rtk_dead_reckoning.cpp
   src/rtk_heading.cpp
   src/velocity_estimator.cpp
+  src/rtkfix_plane_validation.cpp
   include/eagleye_navigation/eagleye_navigation.hpp
+  include/eagleye_navigation/rtkfix_plane_validation.hpp
 )
 
 ament_auto_find_build_dependencies()

eagleye_core/navigation/include/eagleye_navigation/rtkfix_plane_validation.hpp

@@ -0,0 +1,93 @@
+// Copyright (c) 2023, Map IV, Inc.
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+// * Redistributions of source code must retain the above copyright notice,
+//   this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright notice,
+//   this list of conditions and the following disclaimer in the documentation
+//   and/or other materials provided with the distribution.
+// * Neither the name of the Map IV, Inc. nor the names of its contributors
+//   may be used to endorse or promote products derived from this software
+//   without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL COPYRIGHT HOLDER BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#ifndef RTKFIX_PLANE_VALIDATION_HPP
+#define RTKFIX_PLANE_VALIDATION_HPP
+
+#include <yaml-cpp/yaml.h>
+
+#include <sensor_msgs/msg/nav_sat_fix.hpp>
+#include <nmea_msgs/msg/gpgga.hpp>
+#include <geometry_msgs/msg/vector3_stamped.hpp>
+#include <eagleye_msgs/msg/distance.hpp>
+
+#include <rclcpp/rclcpp.hpp>
+
+// RtkfixPlaneValidation
+struct RtkfixPlaneValidationParameter
+{
+  double validation_minimum_interval;
+  double validation_maximum_interval;
+  double outlier_threshold;
+
+  void load(const std::string& yaml_file)
+  {
+    try
+    {
+      YAML::Node conf = YAML::LoadFile(yaml_file);
+
+      this->validation_minimum_interval = conf["/**"]["ros__parameters"]["rtkfix_plane_validation"]["validation_minimum_interval"].as<double>();
+      this->validation_maximum_interval = conf["/**"]["ros__parameters"]["rtkfix_plane_validation"]["validation_maximum_interval"].as<double>();
+      this->outlier_threshold = conf["/**"]["ros__parameters"]["rtkfix_plane_validation"]["outlier_threshold"].as<double>();
+    }
+    catch (YAML::Exception& e)
+    {
+      std::cerr << "\033[1;31mRtkfixPlaneValidationParameter YAML Error: " << e.msg << "\033[m" << std::endl;
+      exit(3);
+    }
+  }
+};
+
+struct RtkfixPlaneValidationStatus
+{
+  bool is_estimated_now;
+  bool is_estimation_started;
+  bool is_estimation_reliable;
+  sensor_msgs::msg::NavSatFix fix_msg;
+};
+
+class RtkfixPlaneValidationEstimator
+{
+public:
+  RtkfixPlaneValidationEstimator();
+  void setParameter(const RtkfixPlaneValidationParameter& param);
+  RtkfixPlaneValidationStatus estimate(const nmea_msgs::msg::Gpgga gga, const eagleye_msgs::msg::Distance distance, const geometry_msgs::msg::Vector3Stamped enu_vel);
+
+private:
+//   // Param
+  RtkfixPlaneValidationParameter param_;
+
+  double gga_time_last_;
+  bool is_first_epoch_;
+
+  std::vector<nmea_msgs::msg::Gpgga> gga_buffer_;
+  std::vector<bool> gga_update_buffer_;
+  std::vector<bool> reliable_status_buffer_;
+  std::vector<bool> unreliable_status_buffer_;
+  std::vector<double> distance_buffer_;
+  std::vector<geometry_msgs::msg::Vector3Stamped> enu_vel_buffer_;
+};
+
+#endif /* RTKFIX_PLANE_VALIDATION_HPP */

eagleye_core/navigation/src/rtkfix_plane_validation.cpp

@@ -0,0 +1,185 @@
+// Copyright (c) 2023, Map IV, Inc.
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+// * Redistributions of source code must retain the above copyright notice,
+//   this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright notice,
+//   this list of conditions and the following disclaimer in the documentation
+//   and/or other materials provided with the distribution.
+// * Neither the name of the Map IV, Inc. nor the names of its contributors
+//   may be used to endorse or promote products derived from this software
+//   without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL COPYRIGHT HOLDER BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+/*
+ * rtkfix_plane_validation.cpp
+ * Author MapIV Takanose
+ */
+
+#include "eagleye_navigation/rtkfix_plane_validation.hpp"
+#include "eagleye_coordinate/eagleye_coordinate.hpp"
+
+#include <numeric>
+
+RtkfixPlaneValidationEstimator::RtkfixPlaneValidationEstimator()
+{
+  // Initialization
+  is_first_epoch_ = true;
+  gga_time_last_ = 0;
+}
+
+void RtkfixPlaneValidationEstimator::setParameter(const RtkfixPlaneValidationParameter& param)
+{
+  // Param
+  param_ = param;
+  // std::cout << "***debug-code: param_.validation_minimum_interval: " << param_.validation_minimum_interval << std::endl;
+  // std::cout << "***debug-code: param_.validation_maximum_interval: " << param_.validation_maximum_interval << std::endl;
+  // std::cout << "***debug-code: param_.outlier_threshold: " << param_.outlier_threshold << std::endl;
+}
+
+RtkfixPlaneValidationStatus RtkfixPlaneValidationEstimator::estimate(const nmea_msgs::msg::Gpgga gga, const eagleye_msgs::msg::Distance distance, const geometry_msgs::msg::Vector3Stamped enu_vel)
+{
+  RtkfixPlaneValidationStatus status;
+  status.is_estimation_started = false;
+  status.is_estimated_now = false;
+  status.is_estimation_reliable = false;
+
+  // buffering
+  rclcpp::Time gga_timestamp_now(gga.header.stamp);
+  double gga_time_now = gga_timestamp_now.seconds();
+  bool gga_update_flag = false;
+  if(gga_time_last_ != gga_time_now && gga.gps_qual == 4)
+  {
+    gga_update_flag = true;
+    gga_time_last_ = gga_time_now;
+  }
+
+  bool reliable_status = false;
+  bool unreliable_status = false;
+  gga_buffer_.push_back(gga);
+  gga_update_buffer_.push_back(gga_update_flag);
+  distance_buffer_.push_back(distance.distance);
+  enu_vel_buffer_.push_back(enu_vel);
+  reliable_status_buffer_.push_back(reliable_status);
+  unreliable_status_buffer_.push_back(unreliable_status);
+
+  if(is_first_epoch_)
+  {
+    is_first_epoch_ = false;
+    return status;
+  }
+
+  double distance_start = distance_buffer_.front();
+  double distance_end = distance_buffer_.back();
+
+  if(distance_end - distance_start < param_.validation_minimum_interval) return status;
+
+  while(1)
+  {
+    distance_start = distance_buffer_.front();
+    distance_end = distance_buffer_.back();
+    if(distance_end - distance_start < param_.validation_maximum_interval) break;
+
+    gga_buffer_.erase(gga_buffer_.begin());
+    gga_update_buffer_.erase(gga_update_buffer_.begin());
+    distance_buffer_.erase(distance_buffer_.begin());
+    enu_vel_buffer_.erase(enu_vel_buffer_.begin());
+    reliable_status_buffer_.erase(reliable_status_buffer_.begin());
+    unreliable_status_buffer_.erase(unreliable_status_buffer_.begin());
+  }
+
+  // Search for past Fix solutions
+  if(!gga_update_flag) return status;
+
+  int buffer_length = distance_buffer_.size();
+  int index_start;
+  for(int iter = 0; iter < buffer_length; iter++)
+  {
+    index_start = buffer_length-1 -iter;
+    if(distance_end - distance_buffer_[index_start] < param_.validation_minimum_interval) continue;
+    if(gga_update_buffer_[index_start] && !unreliable_status_buffer_[index_start]) break;
+  }
+  if(!gga_update_buffer_[index_start]) return status;
+
+  // Fix solution verification by dead reckoning
+  double init_llh_pos[3];
+  double init_enu_pos[3];
+  double init_ecef_base_pos[3];
+  init_llh_pos[0] = gga_buffer_[index_start].lat *M_PI/180;
+  init_llh_pos[1] = gga_buffer_[index_start].lon *M_PI/180;
+  init_llh_pos[2] = gga_buffer_[index_start].alt + gga_buffer_[index_start].undulation;
+  llh2xyz(init_llh_pos,init_ecef_base_pos);
+  xyz2enu(init_ecef_base_pos, init_ecef_base_pos, init_enu_pos);
+
+  double relative_position_x = init_enu_pos[0];
+  double relative_position_y = init_enu_pos[1];
+  for(int iter = index_start+1; iter < buffer_length; iter++)
+  {
+    rclcpp::Time t1(enu_vel_buffer_[iter].header.stamp);
+    rclcpp::Time t0(enu_vel_buffer_[iter-1].header.stamp);
+    double dt = t1.seconds() - t0.seconds();
+    relative_position_x = relative_position_x + enu_vel_buffer_[iter].vector.x * dt;
+    relative_position_y = relative_position_y + enu_vel_buffer_[iter].vector.y * dt;
+  }
+
+  double target_llh_pos[3];
+  double target_enu_pos[3];
+  double target_ecef_pos[3];
+  target_llh_pos[0] = gga_buffer_[buffer_length-1].lat *M_PI/180;
+  target_llh_pos[1] = gga_buffer_[buffer_length-1].lon *M_PI/180;
+  target_llh_pos[2] = gga_buffer_[buffer_length-1].alt + gga_buffer_[buffer_length-1].undulation;
+  llh2xyz(target_llh_pos,target_ecef_pos);
+  xyz2enu(target_ecef_pos, init_ecef_base_pos, target_enu_pos);
+
+  double residual_x = target_enu_pos[0] - relative_position_x;
+  double residual_y = target_enu_pos[1] - relative_position_y;
+  double residual_2d = std::sqrt(residual_x*residual_x + residual_y*residual_y);
+
+  if(residual_2d > param_.outlier_threshold)
+  {
+    unreliable_status_buffer_[buffer_length-1] = true;
+    return status;
+  } 
+
+  if(reliable_status_buffer_[index_start])
+  {
+    status.fix_msg.header = gga_buffer_[buffer_length-1].header;
+    status.fix_msg.latitude = gga_buffer_[buffer_length-1].lat;
+    status.fix_msg.longitude = gga_buffer_[buffer_length-1].lon;
+    status.fix_msg.altitude = gga_buffer_[buffer_length-1].alt + gga_buffer_[buffer_length-1].undulation;
+    status.is_estimation_reliable = true;
+  }
+  reliable_status_buffer_[buffer_length-1] = true;
+
+  // Debug 
+  // rclcpp::Time time_now(enu_vel.header.stamp);
+  // rclcpp::Time target_time(gga_buffer_[buffer_length-1].header.stamp);
+  // rclcpp::Time initial_time(gga_buffer_[index_start].header.stamp);
+  // std::cout << std::setprecision(std::numeric_limits<double>::max_digits10);
+  // std::cout << "***debug-code: RtkfixPlaneValidationEstimator::estimate: " << time_now.seconds() << std::endl;
+  // std::cout << "***debug-code: Target gga time:               " << target_time.seconds() <<  std::endl;
+  // std::cout << "***debug-code: initial gga time:              " << initial_time.seconds() <<  std::endl;
+  // std::cout << "***debug-code: index_start:                   " << index_start <<  std::endl;
+  // std::cout << "***debug-code: gga_update_buffer_ init:       " << (gga_update_buffer_[index_start] ? "\033[1;32mTrue\033[m" : "\033[1;31mFalse\033[m") << std::endl;
+  // std::cout << "***debug-code: distance interval:             " << distance_buffer_[buffer_length-1] - distance_buffer_[index_start] << std::endl;
+  // std::cout << "***debug-code: residual:                      " << residual_2d <<  std::endl;
+  // std::cout << "***debug-code: unreliable_status_buffer init: " << (unreliable_status_buffer_[index_start] ? "\033[1;32mTrue\033[m" : "\033[1;31mFalse\033[m") << std::endl;
+  // std::cout << "***debug-code: reliable_status_buffer init:   " << (reliable_status_buffer_[index_start] ? "\033[1;32mTrue\033[m" : "\033[1;31mFalse\033[m") << std::endl;
+  // std::cout << "***debug-code: reliable_status_buffer last:   " << (reliable_status_buffer_[buffer_length-1] ? "\033[1;32mTrue\033[m" : "\033[1;31mFalse\033[m") << std::endl;
+  // std::cout << "***debug-code: is_estimation_reliable:        " << (status.is_estimation_reliable ? "\033[1;32mTrue\033[m" : "\033[1;31mFalse\033[m") << std::endl;
+  // std::cout <<  std::endl;
+
+  return status;
+}

eagleye_rt/CMakeLists.txt

@@ -110,7 +110,11 @@ ament_auto_add_executable(velocity_estimator
   src/velocity_estimator_node.cpp
 )
 
-install(TARGETS 
+ament_auto_add_executable(rtkfix_plane_validation
+  src/rtkfix_plane_validation_node.cpp
+)
+
+install(TARGETS
   tf_converted_imu
   correction_imu
   distance
@@ -129,6 +133,7 @@ install(TARGETS
   angular_velocity_offset_stop
   enable_additional_rolling
   rolling
+  rtkfix_plane_validation
   DESTINATION lib/$(PROJECT_NAME)
 )
 

eagleye_rt/config/README.md

@@ -280,3 +280,11 @@ Figure shows the relationship between these parameters.
 | gnss_receiving_threshold            | double | Threshold of minimum GNSS reception rate (Value from 0~1)                       | 0.2                  |
 | outlier_threshold                   | double | Outlier threshold due to GNSS multipath [m/s]                                   | 0.1                  |
 | outlier_ratio_threshold             | double | Ratio of allowable outliers in the interval (Value from 0~1)                    | 0.5                  |
+
+### rtkfix_plane_validation
+
+| Name                                | Type   | Description                                                                     | Default value        |
+| :---------------------------------- | :----- | :------------------------------------------------------------------------------ | :------------------- |
+| validation_minimum_interval         | double | Minimum length of DR used for the validation [m]                                | 5                    |
+| validation_maximum_interval         | double | Maximum length of DR used for the validation [m]                                | 10                   |
+| outlier_threshold                   | double | Threshold to recognize the Fix solution as an outlier. [m]                      | 0.3                  |

eagleye_rt/config/eagleye_config.yaml

@@ -172,3 +172,8 @@
         gnss_receiving_threshold: 0.2
         outlier_ratio_threshold: 0.5
         outlier_threshold: 0.1
+
+    rtkfix_plane_validation:
+      validation_minimum_interval: 5
+      validation_maximum_interval: 10
+      outlier_threshold: 0.3

eagleye_rt/launch/eagleye_rt.launch.xml

@@ -135,8 +135,12 @@
     <include file="$(find-pkg-share eagleye_gnss_converter)/launch/gnss_converter.xml">
             <arg name="config_yaml" value="$(var config_yaml)"/>
     </include>
-  </group>
 
+    <node pkg="eagleye_rt" name="rtkfix_plane_validation" exec="rtkfix_plane_validation">
+                <param from="$(var config_path)"/>
+                <param name="yaml_file" value="$(var config_path)"/>
+    </node>
+  </group>
 
 
   <include file="$(find-pkg-share eagleye_tf)/launch/tf.xml" if="$(var use_tf)"/>