Develop ros1 (#216)

* support for nmea and canless mode (#190) * Feature/faster evaluation process (#192) * Accelerate time synchronization process * Delete out of range data before sync * Fixed problem with imu topic name not being retrieved (#193) * Feature/covariance fix dr mode (#200) * add variance to msgs * add variance calc function * support for pose_with_covariance * add covariance output * add covariance to Twist * Add new geoid-model(ROS1) (#204) * Add new geoid-model * Add geographiclib in README.md * Sync eagleye ros2v (#209) * Add gnss_converter(ROS1) (#210) * Add gnss_converter * Publish rmc & gga * Update fix2pose.cpp * Delete nmea2fix (#211) * Delete nmea2fix * gnss_converter support for eagleye_pp * Update paremeter description (#214) * Feature/ros1/document update (#217) * Update paremeter description * Update param description about civariance output --------- Co-authored-by: YutaHoda <[email protected]> Co-authored-by: Aoki-Takanose <[email protected]>
MapIV · Feb 3, 2023 · 6290a57 · 6290a57
1 parent 54d1c04
commit 6290a57
Show file tree

Hide file tree

Showing 64 changed files with 1,001 additions and 555 deletions.
diff --git a/.gitmodules b/.gitmodules
@@ -5,3 +5,6 @@
 	path = eagleye_pp/common/multi_rosbag_controller
 	url = https://github.com/MapIV/multi_rosbag_controller.git
 
+[submodule "eagleye_util/llh_converter"]
+	path = eagleye_util/llh_converter
+	url = https://github.com/MapIV/llh_converter
diff --git a/README.md b/README.md
@@ -57,6 +57,10 @@ Clone and build the necessary packages for Eagleye.
 	git clone https://github.com/MapIV/nmea_ros_bridge.git
 	git clone https://github.com/MapIV/gnss_compass_ros.git
 	git clone https://github.com/MapIV/llh_converter.git
+	sudo apt-get install -y libgeographic-dev geographiclib-tools geographiclib-doc
+	sudo geographiclib-get-geoids best
+	sudo mkdir /usr/share/GSIGEO
+	sudo cp eagleye/eagleye_util/llh_converter/data/gsigeo2011_ver2_1.asc /usr/share/GSIGEO/
 	cd ..
 	rosdep install --from-paths src --ignore-src -r -y
 	catkin_make -DCMAKE_BUILD_TYPE=Release

diff --git a/eagleye_core/navigation/CMakeLists.txt b/eagleye_core/navigation/CMakeLists.txt
@@ -4,6 +4,9 @@ project(eagleye_navigation)
 find_package(PkgConfig REQUIRED)
 pkg_check_modules(YAML_CPP REQUIRED yaml-cpp)
 
+find_package(Eigen3 QUIET)
+set(EIGEN3_INCLUDE_DIRS ${EIGEN3_INCLUDE_DIR})
+
 find_package(catkin REQUIRED COMPONENTS
   roscpp
   std_msgs
@@ -23,6 +26,7 @@ include_directories(
   include
   ${PROJECT_SOURCE_DIR}/include
   ${catkin_INCLUDE_DIRS}
+  ${EIGEN3_INCLUDE_DIRS}
   ${YAML_CPP_INCLUDE_DIRS}
 )
 

diff --git a/eagleye_core/navigation/include/navigation/navigation.hpp b/eagleye_core/navigation/include/navigation/navigation.hpp
@@ -24,10 +24,12 @@
 // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #include <yaml-cpp/yaml.h>
+#include <Eigen/Dense>
 
 #include "sensor_msgs/Imu.h"
 #include "sensor_msgs/NavSatFix.h"
 #include "geometry_msgs/TwistStamped.h"
+#include "geometry_msgs/TwistWithCovarianceStamped.h"
 #include "geometry_msgs/Vector3Stamped.h"
 #include "nmea_msgs/Gpgga.h"
 #include "nmea_msgs/Gprmc.h"
@@ -136,6 +138,7 @@ struct HeadingParameter
   double outlier_threshold;
   double outlier_ratio_threshold;
   double curve_judgment_threshold;
+  double init_STD;
 };
 
 struct HeadingStatus
@@ -194,6 +197,7 @@ struct HeadingInterpolateParameter
   double imu_rate;
   double stop_judgment_threshold;
   double sync_search_period;
+  double proc_noise;
 };
 
 struct HeadingInterpolateStatus
@@ -206,6 +210,7 @@ struct HeadingInterpolateStatus
   double provisional_heading_angle;
   std::vector<double> provisional_heading_angle_buffer;
   std::vector<double> imu_stamp_buffer;
+  double heading_variance_last;
 };
 
 struct PositionParameter
@@ -321,6 +326,10 @@ struct TrajectoryParameter
 {
   double stop_judgment_threshold;
   double curve_judgment_threshold;
+  double sensor_noise_velocity;
+  double sensor_scale_noise_velocity;
+  double sensor_noise_yawrate;
+  double sensor_bias_noise_yawrate;
 };
 
 struct TrajectoryStatus
@@ -411,6 +420,8 @@ struct RtkDeadreckoningParameter
   double tf_gnss_rotation_w;
   std::string tf_gnss_parent_frame;
   std::string tf_gnss_child_frame;
+  double rtk_fix_STD;
+  double proc_noise;
 };
 
 struct RtkDeadreckoningStatus
@@ -428,6 +439,7 @@ struct RtkDeadreckoningStatus
   std::vector<double> provisional_enu_pos_y_buffer;
   std::vector<double> provisional_enu_pos_z_buffer;
   std::vector<double> imu_stamp_buffer;
+  Eigen::MatrixXd position_covariance_last;
 };
 
 struct EnableAdditionalRollingParameter
@@ -504,7 +516,7 @@ extern void smoothing_estimate(const rtklib_msgs::RtklibNav,const geometry_msgs:
   eagleye_msgs::Position*);
 extern void trajectory_estimate(const sensor_msgs::Imu,const geometry_msgs::TwistStamped,const eagleye_msgs::StatusStamped,const eagleye_msgs::Heading,const eagleye_msgs::YawrateOffset,
   const eagleye_msgs::YawrateOffset,const TrajectoryParameter,TrajectoryStatus*,geometry_msgs::Vector3Stamped*,eagleye_msgs::Position*,
-  geometry_msgs::TwistStamped*);
+  geometry_msgs::TwistStamped*, geometry_msgs::TwistWithCovarianceStamped*);
 extern void heading_interpolate_estimate(const sensor_msgs::Imu,const geometry_msgs::TwistStamped,const eagleye_msgs::YawrateOffset,
   const eagleye_msgs::YawrateOffset,const eagleye_msgs::Heading,const eagleye_msgs::SlipAngle,const HeadingInterpolateParameter,HeadingInterpolateStatus*,
   eagleye_msgs::Heading*);
@@ -514,7 +526,7 @@ extern void pitching_estimate(const sensor_msgs::Imu,const nmea_msgs::Gpgga,cons
   const HeightParameter,HeightStatus*,eagleye_msgs::Height*,eagleye_msgs::Pitching*,eagleye_msgs::AccXOffset*,eagleye_msgs::AccXScaleFactor*);
 extern void trajectory3d_estimate(const sensor_msgs::Imu,const geometry_msgs::TwistStamped,const eagleye_msgs::StatusStamped,const eagleye_msgs::Heading,
   const eagleye_msgs::YawrateOffset,const eagleye_msgs::YawrateOffset,const eagleye_msgs::Pitching,const TrajectoryParameter,TrajectoryStatus*,
-  geometry_msgs::Vector3Stamped*,eagleye_msgs::Position*,geometry_msgs::TwistStamped*);
+  geometry_msgs::Vector3Stamped*,eagleye_msgs::Position*,geometry_msgs::TwistStamped*, geometry_msgs::TwistWithCovarianceStamped*);
 extern void angular_velocity_offset_stop_estimate(const geometry_msgs::TwistStamped, const sensor_msgs::Imu, const AngularVelocityOffsetStopParameter,
   AngularVelocityOffsetStopStatus*, eagleye_msgs::AngularVelocityOffset*);
 extern void rtk_deadreckoning_estimate(const rtklib_msgs::RtklibNav,const geometry_msgs::Vector3Stamped,const nmea_msgs::Gpgga, const eagleye_msgs::Heading,

diff --git a/eagleye_core/navigation/src/heading.cpp b/eagleye_core/navigation/src/heading.cpp
@@ -141,10 +141,10 @@ void heading_estimate_(sensor_msgs::Imu imu,geometry_msgs::TwistStamped velocity
       std::vector<double> base_heading_angle_buffer2;
       std::vector<double> diff_buffer;
 
-     if(!heading_interpolate.status.enabled_status)
-     {
-       heading_interpolate.heading_angle = heading_status->heading_angle_buffer [index[index_length-1]];
-     }
+      if(!heading_interpolate.status.enabled_status)
+      {
+        heading_interpolate.heading_angle = heading_status->heading_angle_buffer [index[index_length-1]];
+      }
 
       int ref_cnt;
       std::vector<double> heading_angle_buffer2;
@@ -229,6 +229,9 @@ void heading_estimate_(sensor_msgs::Imu imu,geometry_msgs::TwistStamped velocity
             provisional_heading_angle_buffer[index[index_length-1]]);
         }
         heading->status.estimate_status = true;
+
+        double heading_STD = heading_parameter.init_STD; 
+        heading->variance = heading_STD*heading_STD;
       }
     }
   }

diff --git a/eagleye_core/navigation/src/heading_interpolate.cpp b/eagleye_core/navigation/src/heading_interpolate.cpp
@@ -43,6 +43,8 @@ void heading_interpolate_estimate(const sensor_msgs::Imu imu, const geometry_msg
   bool heading_estimate_status = false;
   std::size_t imu_stamp_buffer_length;
 
+  double proc_noise = heading_interpolate_parameter.proc_noise;
+
   double search_buffer_number = heading_interpolate_parameter.sync_search_period * heading_interpolate_parameter.imu_rate;
 
   yawrate = imu.angular_velocity.z;
@@ -77,6 +79,7 @@ void heading_interpolate_estimate(const sensor_msgs::Imu imu, const geometry_msg
   {
     double dt = imu.header.stamp.toSec() - heading_interpolate_status->time_last;
     heading_interpolate_status->provisional_heading_angle += yawrate * dt;
+    heading_interpolate_status->heading_variance_last += proc_noise*proc_noise;
   }
 
   // data buffer generate
@@ -107,22 +110,26 @@ void heading_interpolate_estimate(const sensor_msgs::Imu imu, const geometry_msg
     if (heading_estimate_status && estimate_index > 0 && heading_interpolate_status->number_buffer >= estimate_index &&
       heading_interpolate_status->heading_estimate_status_count > 1)
     {
+      double heading_variance = heading.variance;
       double diff_estimate_heading_angle = (heading_interpolate_status->provisional_heading_angle_buffer[estimate_index-1] - heading.heading_angle);
       for (int i = estimate_index; i <= heading_interpolate_status->number_buffer; i++)
       {
         heading_interpolate_status->provisional_heading_angle_buffer[i-1] -= diff_estimate_heading_angle;
+        heading_variance += proc_noise*proc_noise;
       }
       heading_interpolate_status->provisional_heading_angle =
         heading_interpolate_status->provisional_heading_angle_buffer[heading_interpolate_status->number_buffer-1];
 
       heading_interpolate->status.enabled_status = true;
       heading_interpolate->status.estimate_status = true;
+      heading_interpolate_status->heading_variance_last = heading_variance;
     }
     else if (heading_interpolate_status->heading_estimate_status_count == 1)
     {
       heading_interpolate_status->provisional_heading_angle = heading.heading_angle;
       heading_interpolate->status.enabled_status = true;
       heading_interpolate->status.estimate_status = false;
+      heading_interpolate_status->heading_variance_last = heading.variance;
     }
     else
     {
@@ -133,6 +140,7 @@ void heading_interpolate_estimate(const sensor_msgs::Imu imu, const geometry_msg
   if (heading_interpolate_status->heading_estimate_start_status)
   {
     heading_interpolate->heading_angle = heading_interpolate_status->provisional_heading_angle + slip_angle.slip_angle;
+    heading_interpolate->variance = heading_interpolate_status->heading_variance_last;
   }
   else
   {

diff --git a/eagleye_core/navigation/src/rtk_deadreckoning.cpp b/eagleye_core/navigation/src/rtk_deadreckoning.cpp
@@ -41,7 +41,7 @@ void rtk_deadreckoning_estimate_(geometry_msgs::Vector3Stamped enu_vel, nmea_msg
   double ecef_rtk[3];
   double llh_pos[3],llh_rtk[3];
 
-  if(rtk_deadreckoning_status->position_estimate_start_status)
+  if(rtk_deadreckoning_status->position_estimate_start_status && heading.status.enabled_status)
   {
     ecef_base_pos[0] = enu_absolute_rtk_deadreckoning->ecef_base_pos.x;
     ecef_base_pos[1] = enu_absolute_rtk_deadreckoning->ecef_base_pos.y;
@@ -97,24 +97,76 @@ void rtk_deadreckoning_estimate_(geometry_msgs::Vector3Stamped enu_vel, nmea_msg
       enu_absolute_rtk_deadreckoning->status.enabled_status = true;
       enu_absolute_rtk_deadreckoning->status.estimate_status = false;
     }
+    else if(!enu_absolute_rtk_deadreckoning->status.enabled_status)
+    {
+      return;
+    }
 
     enu_pos[0] = rtk_deadreckoning_status->provisional_enu_pos_x;
     enu_pos[1] = rtk_deadreckoning_status->provisional_enu_pos_y;
     enu_pos[2] = rtk_deadreckoning_status->provisional_enu_pos_z;
 
     enu2llh(enu_pos, ecef_base_pos, llh_pos);
 
+    double rtk_fix_STD = rtk_deadreckoning_parameter.rtk_fix_STD;
+    Eigen::MatrixXd init_covariance;
+    init_covariance = Eigen::MatrixXd::Zero(6, 6);
+    init_covariance(0,0) = rtk_fix_STD * rtk_fix_STD;
+    init_covariance(1,1) = rtk_fix_STD * rtk_fix_STD;
+    init_covariance(2,2) = rtk_fix_STD * rtk_fix_STD;
+    init_covariance(5,5) = heading.variance;
+
+    double proc_noise = rtk_deadreckoning_parameter.proc_noise;
+    Eigen::MatrixXd proc_covariance;
+    proc_covariance = Eigen::MatrixXd::Zero(6, 6);
+    proc_covariance(0,0) = proc_noise * proc_noise;
+    proc_covariance(1,1) = proc_noise * proc_noise;
+    proc_covariance(2,2) = proc_noise * proc_noise;
+
+    Eigen::MatrixXd position_covariance;
+    position_covariance = Eigen::MatrixXd::Zero(6, 6);
+
+
+    if(enu_absolute_rtk_deadreckoning->status.estimate_status)
+    {
+      position_covariance = init_covariance;
+      rtk_deadreckoning_status->position_covariance_last = position_covariance;
+    }
+    else
+    {
+      Eigen::MatrixXd jacobian;
+      jacobian = Eigen::MatrixXd::Zero(6, 6);
+      jacobian(0,0) = 1;
+      jacobian(1,1) = 1;
+      jacobian(2,2) = 1;
+      jacobian(3,3) = 1;
+      jacobian(4,4) = 1;
+      jacobian(5,5) = 1;
+      jacobian(0,5) = enu_vel.vector.y*(enu_vel.header.stamp.toSec() - rtk_deadreckoning_status->time_last);
+      jacobian(1,5) = -enu_vel.vector.x*(enu_vel.header.stamp.toSec() - rtk_deadreckoning_status->time_last);
+
+      // MEMO: Jacobean not included
+      // position_covariance = rtk_deadreckoning_status->position_covariance_last + proc_covariance;
+
+      // MEMO: Jacobean not included
+      position_covariance = jacobian * rtk_deadreckoning_status->position_covariance_last * (jacobian.transpose())   + proc_covariance;
+
+      rtk_deadreckoning_status->position_covariance_last = position_covariance;
+    }
+
     eagleye_fix->latitude = llh_pos[0] * 180/M_PI;
     eagleye_fix->longitude = llh_pos[1] * 180/M_PI;
     eagleye_fix->altitude = llh_pos[2];
-    // TODO(Map IV): temporary covariance value
-    eagleye_fix->position_covariance[0] = (enu_absolute_rtk_deadreckoning->status.estimate_status) ? 0.02 * 0.02 : 1.5 * 1.5; // [m^2]
-    eagleye_fix->position_covariance[4] = (enu_absolute_rtk_deadreckoning->status.estimate_status) ? 0.02 * 0.02 : 1.5 * 1.5; // [m^2]
-    eagleye_fix->position_covariance[8] = (enu_absolute_rtk_deadreckoning->status.estimate_status) ? 0.02 * 0.02 : 1.5 * 1.5; // [m^2]
+    eagleye_fix->position_covariance[0] = position_covariance(0,0); // [m^2]
+    eagleye_fix->position_covariance[4] = position_covariance(1,1); // [m^2]
+    eagleye_fix->position_covariance[8] = position_covariance(2,2); // [m^2]
 
     enu_absolute_rtk_deadreckoning->enu_pos.x = enu_pos[0];
     enu_absolute_rtk_deadreckoning->enu_pos.y = enu_pos[1];
     enu_absolute_rtk_deadreckoning->enu_pos.z = enu_pos[2];
+    enu_absolute_rtk_deadreckoning->covariance[0] = position_covariance(0,0); // [m^2]
+    enu_absolute_rtk_deadreckoning->covariance[4] = position_covariance(1,1); // [m^2]
+    enu_absolute_rtk_deadreckoning->covariance[8] = position_covariance(2,2); // [m^2]
 
     rtk_deadreckoning_status->time_last = enu_vel.header.stamp.toSec();
     rtk_deadreckoning_status->position_stamp_last = gga.header.stamp.toSec();

diff --git a/eagleye_core/navigation/src/trajectory.cpp b/eagleye_core/navigation/src/trajectory.cpp
@@ -31,26 +31,64 @@
 #include "coordinate/coordinate.hpp"
 #include "navigation/navigation.hpp"
 
+
+void calculate_covariance(const geometry_msgs::TwistStamped velocity, const eagleye_msgs::StatusStamped velocity_status,
+  const eagleye_msgs::YawrateOffset yawrate_offset_stop, const TrajectoryParameter trajectory_parameter,
+  geometry_msgs::TwistWithCovarianceStamped* eagleye_twist_with_covariance)
+{
+  double noise_velocity;
+  double noise_yawrate;
+
+  if(velocity_status.status.enabled_status)
+  {
+    noise_velocity = trajectory_parameter.sensor_noise_velocity * trajectory_parameter.sensor_noise_velocity;
+  }
+  else
+  {
+    noise_velocity = trajectory_parameter.sensor_noise_velocity * trajectory_parameter.sensor_noise_velocity
+      + (velocity.twist.linear.x*trajectory_parameter.sensor_scale_noise_velocity)*(velocity.twist.linear.x*trajectory_parameter.sensor_scale_noise_velocity);
+  }
+
+  if(yawrate_offset_stop.status.enabled_status)
+  {
+    noise_yawrate = trajectory_parameter.sensor_noise_yawrate * trajectory_parameter.sensor_noise_yawrate;
+  }
+  else
+  {
+    noise_yawrate = trajectory_parameter.sensor_noise_yawrate * trajectory_parameter.sensor_noise_yawrate
+      + trajectory_parameter.sensor_bias_noise_yawrate * trajectory_parameter.sensor_bias_noise_yawrate;
+  }
+
+  eagleye_twist_with_covariance->twist.covariance[0] = noise_velocity;
+  eagleye_twist_with_covariance->twist.covariance[35] = noise_yawrate;
+
+}
+
 void trajectory_estimate(const sensor_msgs::Imu imu, const geometry_msgs::TwistStamped velocity, 
   const eagleye_msgs::StatusStamped velocity_status, const eagleye_msgs::Heading heading_interpolate_3rd, 
   const eagleye_msgs::YawrateOffset yawrate_offset_stop, const eagleye_msgs::YawrateOffset yawrate_offset_2nd, 
   const TrajectoryParameter trajectory_parameter, TrajectoryStatus* trajectory_status, geometry_msgs::Vector3Stamped* enu_vel, 
-  eagleye_msgs::Position* enu_relative_pos, geometry_msgs::TwistStamped* eagleye_twist)
+  eagleye_msgs::Position* enu_relative_pos, geometry_msgs::TwistStamped* eagleye_twist, geometry_msgs::TwistWithCovarianceStamped* eagleye_twist_with_covariance)
 {
 
   if (std::abs(velocity.twist.linear.x) > trajectory_parameter.stop_judgment_threshold &&
   yawrate_offset_2nd.status.enabled_status == true)
   {
     // Inverted because the coordinate system is reversed
     eagleye_twist->twist.angular.z = -1 * (imu.angular_velocity.z + yawrate_offset_2nd.yawrate_offset);
+    eagleye_twist_with_covariance->twist.twist.angular.z = -1 * (imu.angular_velocity.z + yawrate_offset_2nd.yawrate_offset);
   }
   else
   {
     // Inverted because the coordinate system is reversed
     eagleye_twist->twist.angular.z = -1 * (imu.angular_velocity.z + yawrate_offset_stop.yawrate_offset);
+    eagleye_twist_with_covariance->twist.twist.angular.z = -1 * (imu.angular_velocity.z + yawrate_offset_stop.yawrate_offset);
   }
 
   eagleye_twist->twist.linear.x = velocity.twist.linear.x;
+  eagleye_twist_with_covariance->twist.twist.linear.x = velocity.twist.linear.x;
+
+  calculate_covariance(velocity, velocity_status, yawrate_offset_stop, trajectory_parameter, eagleye_twist_with_covariance);
 
   if (trajectory_status->estimate_status_count == 0 && velocity_status.status.enabled_status == true &&
     heading_interpolate_3rd.status.enabled_status == true)
@@ -107,22 +145,28 @@ void trajectory3d_estimate(const sensor_msgs::Imu imu, const geometry_msgs::Twis
   const eagleye_msgs::StatusStamped velocity_status, const eagleye_msgs::Heading heading_interpolate_3rd, 
   const eagleye_msgs::YawrateOffset yawrate_offset_stop, const eagleye_msgs::YawrateOffset yawrate_offset_2nd, 
   const eagleye_msgs::Pitching pitching, const TrajectoryParameter trajectory_parameter, TrajectoryStatus* trajectory_status, 
-  geometry_msgs::Vector3Stamped* enu_vel, eagleye_msgs::Position* enu_relative_pos, geometry_msgs::TwistStamped* eagleye_twist)
+  geometry_msgs::Vector3Stamped* enu_vel, eagleye_msgs::Position* enu_relative_pos, geometry_msgs::TwistStamped* eagleye_twist,
+  geometry_msgs::TwistWithCovarianceStamped* eagleye_twist_with_covariance)
 {
 
   if (std::abs(velocity.twist.linear.x) > trajectory_parameter.stop_judgment_threshold &&
     yawrate_offset_2nd.status.enabled_status == true)
   {
     //Inverted because the coordinate system is reversed
     eagleye_twist->twist.angular.z = -1 * (imu.angular_velocity.z + yawrate_offset_2nd.yawrate_offset);
+    eagleye_twist_with_covariance->twist.twist.angular.z = -1 * (imu.angular_velocity.z + yawrate_offset_2nd.yawrate_offset);
   }
   else
   {
     //Inverted because the coordinate system is reversed
     eagleye_twist->twist.angular.z = -1 * (imu.angular_velocity.z + yawrate_offset_stop.yawrate_offset);
+    eagleye_twist_with_covariance->twist.twist.angular.z = -1 * (imu.angular_velocity.z + yawrate_offset_stop.yawrate_offset);
   }
 
   eagleye_twist->twist.linear.x = velocity.twist.linear.x;
+  eagleye_twist_with_covariance->twist.twist.linear.x = velocity.twist.linear.x;
+
+  calculate_covariance(velocity, velocity_status, yawrate_offset_stop, trajectory_parameter, eagleye_twist_with_covariance);
 
   if (trajectory_status->estimate_status_count == 0 && velocity_status.status.enabled_status == true &&
     heading_interpolate_3rd.status.enabled_status == true)
@@ -166,6 +210,8 @@ void trajectory3d_estimate(const sensor_msgs::Imu imu, const geometry_msgs::Twis
     enu_relative_pos->status.enabled_status = enu_relative_pos->status.estimate_status = true;
   }
 
+
+
   trajectory_status->heading_last = heading_interpolate_3rd.heading_angle;
   trajectory_status->time_last = imu.header.stamp.toSec();
 }
diff --git a/eagleye_msgs/msg/Heading.msg b/eagleye_msgs/msg/Heading.msg
@@ -1,4 +1,5 @@
 Header header
 
 float64 heading_angle
+float64 variance
 eagleye_msgs/Status status