Debugging: Before eskf_predict. IMU Trans Mat OK.

include/eskf_odom_alg_node.h

@@ -54,6 +54,7 @@ class EskfOdomAlgNode : public algorithm_base::IriBaseAlgorithm<EskfOdomAlgorith
 
     bool flying_; // Quadrotor landed or flying information.
     double gnd_dist_; // Ground distance (m) obtained from PX4 optical flow pointing downward.
+    bool is_first_; // First reading should not contribute to propagate nominal (integration requirements)
 
     // [publisher attributes]
 

src/eskf_odom_alg_node.cpp

@@ -18,7 +18,8 @@ EskfOdomAlgNode::EskfOdomAlgNode(void) :
 
   // Initialize ROBOT phase
   this->flying_ = false;
-  this->gnd_dist_ = 0; // a little bit more than the minimum distance PX4 can detect (0.3m)
+  this->gnd_dist_ = 0.0; // a little bit more than the minimum distance PX4 can detect (0.3m)
+  this->is_first_ = true;
 
   // [init publishers]
   
@@ -104,7 +105,7 @@ void EskfOdomAlgNode::read_and_set_ini_params(void)
   this->alg_.set_init_params(f_params,x_state,dx_state,imu_params,px4_params);
 
   // Print already set values of filter and sensors initial parameters
-  this->alg_.print_ini_params();
+  //this->alg_.print_ini_params();
 
   this->alg_.unlock();
 
@@ -161,10 +162,9 @@ void EskfOdomAlgNode::mainNodeThread(void)
   static tf::TransformBroadcaster br;
   tf::Transform transform;
   transform.setOrigin(tf::Vector3(state(0), state(1), state(2)));
-  tf::Quaternion q(state(7),state(8),state(9),state(6)); //ROS:[qx,qy,qz,qw] Filter:[qw,qx,qy,qz]
+  tf::Quaternion q(state(7),state(8),state(9),state(6)); //TF:[qx,qy,qz,qw] Filter:[qw,qx,qy,qz]
   transform.setRotation(q);
   br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", "base_link"));
-
 }
 
 /*  [subscriber callbacks] */
@@ -239,6 +239,7 @@ void EskfOdomAlgNode::imu_callback(const sensor_msgs::Imu::ConstPtr& msg)
   this->imu_mutex_exit(); 
 
   double t = stamp.toSec();
+
   Vector3d a;
   a << ax,ay,az;
   Vector3d w;
@@ -249,9 +250,14 @@ void EskfOdomAlgNode::imu_callback(const sensor_msgs::Imu::ConstPtr& msg)
   // Set values into filter object
   this->alg_.set_imu_reading(t,a,w);
  
-  // Propagate Nominal-State
-  this->alg_.prop_nominal();
-
+  if (this->is_first_)
+    this->is_first_ = false;
+  else 
+  {
+    // Propagate Nominal-State
+    this->alg_.prop_nominal();  
+  }
+  
   this->alg_.unlock(); 
 }