Adding support for absolute position inputs (GPS/LPS systems)

robot_pose_ekf/include/robot_pose_ekf/odom_estimation.h

@@ -69,12 +69,13 @@ class OdomEstimation
   /** update the extended Kalman filter
    * \param odom_active specifies if the odometry sensor is active or not
    * \param imu_active specifies if the imu sensor is active or not
+   * \param gps_active specifies if the gps sensor is active or not
    * \param vo_active specifies if the vo sensor is active or not
    * \param filter_time update the ekf up to this time
    * \param diagnostics_res returns false if the diagnostics found that the sensor measurements are inconsistent
    * returns true on successfull update
    */
-  bool update(bool odom_active, bool imu_active, bool vo_active, const ros::Time& filter_time, bool& diagnostics_res);
+  bool update(bool odom_active, bool imu_active, bool gps_active, bool vo_active, const ros::Time& filter_time, bool& diagnostics_res);
 
   /** initialize the extended Kalman filter
    * \param prior the prior robot pose
@@ -140,16 +141,18 @@ class OdomEstimation
   BFL::LinearAnalyticMeasurementModelGaussianUncertainty* imu_meas_model_;
   BFL::LinearAnalyticConditionalGaussian*                 vo_meas_pdf_;
   BFL::LinearAnalyticMeasurementModelGaussianUncertainty* vo_meas_model_;
+  BFL::LinearAnalyticConditionalGaussian*                 gps_meas_pdf_;
+  BFL::LinearAnalyticMeasurementModelGaussianUncertainty* gps_meas_model_;
   BFL::Gaussian*                                          prior_;
   BFL::ExtendedKalmanFilter*                              filter_;
-  MatrixWrapper::SymmetricMatrix                          odom_covariance_, imu_covariance_, vo_covariance_;
+  MatrixWrapper::SymmetricMatrix                          odom_covariance_, imu_covariance_, vo_covariance_, gps_covariance_;
 
   // vars
   MatrixWrapper::ColumnVector vel_desi_, filter_estimate_old_vec_;
   tf::Transform filter_estimate_old_;
-  tf::StampedTransform odom_meas_, odom_meas_old_, imu_meas_, imu_meas_old_, vo_meas_, vo_meas_old_;
+  tf::StampedTransform odom_meas_, odom_meas_old_, imu_meas_, imu_meas_old_, vo_meas_, vo_meas_old_, gps_meas_, gps_meas_old_;
   ros::Time filter_time_old_;
-  bool filter_initialized_, odom_initialized_, imu_initialized_, vo_initialized_;
+  bool filter_initialized_, odom_initialized_, imu_initialized_, vo_initialized_, gps_initialized_;
 
   // diagnostics
   double diagnostics_odom_rot_rel_, diagnostics_imu_rot_rel_;

robot_pose_ekf/include/robot_pose_ekf/odom_estimation_node.h

@@ -71,6 +71,7 @@ namespace estimation
 typedef boost::shared_ptr<nav_msgs::Odometry const> OdomConstPtr;
 typedef boost::shared_ptr<sensor_msgs::Imu const> ImuConstPtr;
 typedef boost::shared_ptr<nav_msgs::Odometry const> VoConstPtr;
+typedef boost::shared_ptr<nav_msgs::Odometry const> GpsConstPtr;
 typedef boost::shared_ptr<geometry_msgs::Twist const> VelConstPtr;
 
 class OdomEstimationNode
@@ -95,13 +96,17 @@ class OdomEstimationNode
   /// callback function for vo data
   void voCallback(const VoConstPtr& vo);
 
+  /// callback function for vo data
+  void gpsCallback(const GpsConstPtr& gps);
+
+
   /// get the status of the filter
   bool getStatus(robot_pose_ekf::GetStatus::Request& req, robot_pose_ekf::GetStatus::Response& resp);
 
   ros::NodeHandle node_;
   ros::Timer timer_;
   ros::Publisher pose_pub_;
-  ros::Subscriber odom_sub_, imu_sub_, vo_sub_;
+  ros::Subscriber odom_sub_, imu_sub_, vo_sub_,gps_sub_;
   ros::ServiceServer state_srv_;
 
   // ekf filter
@@ -115,25 +120,26 @@ class OdomEstimationNode
   tf::TransformBroadcaster odom_broadcaster_;
 
   // vectors
-  tf::Transform odom_meas_, imu_meas_, vo_meas_;
+  tf::Transform odom_meas_, imu_meas_, vo_meas_, gps_meas_;
   tf::Transform base_vo_init_;
+  tf::Transform base_gps_init_;
   tf::StampedTransform camera_base_;
-  ros::Time odom_time_, imu_time_, vo_time_;
-  ros::Time odom_stamp_, imu_stamp_, vo_stamp_, filter_stamp_;
-  ros::Time odom_init_stamp_, imu_init_stamp_, vo_init_stamp_;
-  bool odom_active_, imu_active_, vo_active_;
-  bool odom_used_, imu_used_, vo_used_;
-  bool odom_initializing_, imu_initializing_, vo_initializing_;
+  ros::Time odom_time_, imu_time_, vo_time_, gps_time_;
+  ros::Time odom_stamp_, imu_stamp_, vo_stamp_, gps_stamp_, filter_stamp_;
+  ros::Time odom_init_stamp_, imu_init_stamp_, vo_init_stamp_, gps_init_stamp_;
+  bool odom_active_, imu_active_, vo_active_, gps_active_;
+  bool odom_used_, imu_used_, vo_used_, gps_used_;
+  bool odom_initializing_, imu_initializing_, vo_initializing_, gps_initializing_;
   double timeout_;
-  MatrixWrapper::SymmetricMatrix odom_covariance_, imu_covariance_, vo_covariance_;
+  MatrixWrapper::SymmetricMatrix odom_covariance_, imu_covariance_, vo_covariance_, gps_covariance_;
   bool debug_, self_diagnose_;
   std::string output_frame_;
 
   // log files for debugging
-  std::ofstream odom_file_, imu_file_, vo_file_, corr_file_, time_file_, extra_file_;
+  std::ofstream odom_file_, imu_file_, vo_file_, gps_file_, corr_file_, time_file_, extra_file_;
 
   // counters
-  unsigned int odom_callback_counter_, imu_callback_counter_, vo_callback_counter_, ekf_sent_counter_;
+  unsigned int odom_callback_counter_, imu_callback_counter_, vo_callback_counter_,gps_callback_counter_, ekf_sent_counter_;
 
 }; // class
 

robot_pose_ekf/plotekf.m

@@ -1,15 +1,53 @@
-load odom_file.txt;
+load /tmp/odom_file.txt;
 %load vo_file.txt;
-load corr_file.txt;
+load /tmp/corr_file.txt;
+load /tmp/gps_file.txt;
+load /tmp/imu_file.txt;
 
-
-figure; hold on;
+figure; 
+hold on;
 axis equal;
-plot(odom_file(:,1), odom_file(:,2),'b');
-%plot(vo_file(:,1),vo_file(:,2),'g');
-plot(corr_file(:,1), corr_file(:,2),'r');
+plot(odom_file(:,2), odom_file(:,3),'b');
+%plot(vo_file(:,2),vo_file(:,3),'g');
+plot(corr_file(:,2), corr_file(:,3),'r');
+plot(gps_file(:,2), gps_file(:,3),'k');
+legend('Wheel Odometry', 'Filter output', 'GPS Measurements');
+%legend('Wheel Odometry','Visual Odometry', 'Filter output', 'GPS Measurements');
+%legend('Wheel Odometry','Visual Odometry', 'Filter output');
 hold off;
 
-error_odom = sqrt( (odom_file(1,1)-odom_file(end,1))^2 + (odom_file(1,2)-odom_file(end,2))^2 )
-%error_vo = sqrt( (vo_file(1,1)-vo_file(end,1))^2 + (vo_file(1,2)-vo_file(end,2))^2 )
-error_corr = sqrt( (corr_file(1,1)-corr_file(end,1))^2 + (corr_file(1,2)-corr_file(end,2))^2 )
+figure; 
+subplot(3,1,1)
+hold on;
+plot(odom_file(:,1),odom_file(:,2),'b');
+%plot(vo_file(:,1),vo_file(:,2),'g');
+plot(corr_file(:,1),corr_file(:,2),'r');
+plot(gps_file(:,1), gps_file(:,2), 'k');
+legend('Wheel Odometry', 'Filter output', 'GPS Measurements');
+%legend('Wheel Odometry','Visual Odometry', 'Filter output', 'GPS Measurements');
+%legend('Wheel Odometry','Visual Odometry', 'Filter output');
+subplot(3,1,2)
+hold on;
+plot(odom_file(:,1),odom_file(:,3),'b');
+%plot(vo_file(:,1),vo_file(:,3),'g');
+plot(corr_file(:,1),corr_file(:,3),'r');
+plot(gps_file(:,1), gps_file(:,3), 'k');
+legend('Wheel Odometry', 'Filter output', 'GPS Measurements');
+%legend('Wheel Odometry','Visual Odometry', 'Filter output', 'GPS Measurements');
+%legend('Wheel Odometry','Visual Odometry', 'Filter output');
+
+subplot(3,1,3)
+hold on;
+plot(odom_file(:,1),odom_file(:,4),'b');
+%plot(vo_file(:,1),vo_file(:,7),'g');
+plot(corr_file(:,1),corr_file(:,7),'r');
+plot(imu_file(:,1), imu_file(:,2), 'k');
+legend('Wheel Odometry', 'Filter output', 'IMU Measurements');
+%legend('Wheel Odometry','Visual Odometry', 'Filter output', 'IMU Measurements');
+
+
+
+error_odom = sqrt( (odom_file(1,2)-odom_file(end,2))^2 + (odom_file(1,3)-odom_file(end,3))^2 )
+%error_vo = sqrt( (vo_file(1,2)-vo_file(end,2))^2 + (vo_file(1,3)-vo_file(end,3))^2 )
+error_corr = sqrt( (corr_file(1,2)-corr_file(end,2))^2 + (corr_file(1,3)-corr_file(end,3))^2 )
+error_gps = sqrt( (gps_file(1,2)-gps_file(end,2))^2 + (gps_file(1,3)-gps_file(end,3))^2 )

robot_pose_ekf/robot_pose_ekf.launch

@@ -1,15 +1,17 @@
 <launch>
 
-<node pkg="robot_pose_ekf" type="robot_pose_ekf" name="robot_pose_ekf">
-  <param name="output_frame" value="odom_combined"/>
+<node pkg="robot_pose_ekf" type="robot_pose_ekf" name="robot_pose_ekf" output="screen">
+  <param name="output_frame" value="odom"/>
   <param name="freq" value="30.0"/>
+  <param name="debug" value="true"/>
   <param name="sensor_timeout" value="1.0"/>  
   <param name="odom_used" value="true"/>
   <param name="imu_used" value="true"/>
-  <param name="vo_used" value="true"/>
+  <param name="vo_used" value="false"/>
+  <param name="gps_used" value="true"/>
 
-  <remap from="odom" to="pr2_base_odometry/odom" />
 </node>
 
+
 </launch>
 

robot_pose_ekf/src/odom_estimation.cpp

@@ -52,7 +52,8 @@ namespace estimation
     filter_initialized_(false),
     odom_initialized_(false),
     imu_initialized_(false),
-    vo_initialized_(false)
+    vo_initialized_(false),
+    gps_initialized_(false)
   {
     // create SYSTEM MODEL
     ColumnVector sysNoise_Mu(6);  sysNoise_Mu = 0;
@@ -91,6 +92,18 @@ namespace estimation
     Hvo(1,1) = 1;    Hvo(2,2) = 1;    Hvo(3,3) = 1;    Hvo(4,4) = 1;    Hvo(5,5) = 1;    Hvo(6,6) = 1;
     vo_meas_pdf_   = new LinearAnalyticConditionalGaussian(Hvo, measurement_Uncertainty_Vo);
     vo_meas_model_ = new LinearAnalyticMeasurementModelGaussianUncertainty(vo_meas_pdf_);
+
+
+
+// create MEASUREMENT MODEL GPS
+    ColumnVector measNoiseGps_Mu(3);  measNoiseGps_Mu = 0;
+    SymmetricMatrix measNoiseGps_Cov(3);  measNoiseGps_Cov = 0;
+    for (unsigned int i=1; i<=3; i++) measNoiseGps_Cov(i,i) = 1;
+    Gaussian measurement_Uncertainty_GPS(measNoiseGps_Mu, measNoiseGps_Cov);
+    Matrix Hgps(3,6);  Hgps = 0;
+    Hgps(1,1) = 1;    Hgps(2,2) = 1;    Hgps(3,3) = 1;    
+    gps_meas_pdf_   = new LinearAnalyticConditionalGaussian(Hgps, measurement_Uncertainty_GPS);
+    gps_meas_model_ = new LinearAnalyticMeasurementModelGaussianUncertainty(gps_meas_pdf_);
   };
 
 
@@ -105,6 +118,8 @@ namespace estimation
     delete imu_meas_pdf_;
     delete vo_meas_model_;
     delete vo_meas_pdf_;
+    delete gps_meas_model_;
+    delete gps_meas_pdf_;
     delete sys_pdf_;
     delete sys_model_;
   };
@@ -141,7 +156,7 @@ namespace estimation
 
 
   // update filter
-  bool OdomEstimation::update(bool odom_active, bool imu_active, bool vo_active, const Time&  filter_time, bool& diagnostics_res)
+  bool OdomEstimation::update(bool odom_active, bool imu_active, bool gps_active, bool vo_active, const Time&  filter_time, bool& diagnostics_res)
   {
     // only update filter when it is initialized
     if (!filter_initialized_){
@@ -254,7 +269,34 @@ namespace estimation
     }
     // sensor not active
     else vo_initialized_ = false;
-
+
+
+
+// process gps measurement
+    // ----------------------
+    if (gps_active){
+      if (!transformer_.canTransform("base_footprint","gps", filter_time)){
+        ROS_ERROR("filter time older than gps message buffer");
+        return false;
+      }
+      transformer_.lookupTransform("gps", "base_footprint", filter_time, gps_meas_);
+      if (gps_initialized_){
+        gps_meas_pdf_->AdditiveNoiseSigmaSet(gps_covariance_ * pow(dt,2));
+        ColumnVector gps_vec(3);
+        double tmp;
+        //Take gps as an absolute measurement, do not convert to relative measurement
+        decomposeTransform(gps_meas_, gps_vec(1), gps_vec(2), gps_vec(3), tmp, tmp, tmp);
+        filter_->Update(gps_meas_model_,  gps_vec);
+      }
+      else {
+        gps_initialized_ = true;
+        gps_meas_old_ = gps_meas_;
+      }
+    }
+    // sensor not active
+    else gps_initialized_ = false;
+
+
 
     // remember last estimate
     filter_estimate_old_vec_ = filter_->PostGet()->ExpectedValueGet();
@@ -301,6 +343,7 @@ namespace estimation
     if (meas.child_frame_id_ == "wheelodom") odom_covariance_ = covar;
     else if (meas.child_frame_id_ == "imu")  imu_covariance_  = covar;
     else if (meas.child_frame_id_ == "vo")   vo_covariance_   = covar;
+    else if (meas.child_frame_id_ == "gps")  gps_covariance_  = covar;
     else ROS_ERROR("Adding a measurement for an unknown sensor %s", meas.child_frame_id_.c_str());
   };