Working. trying to solve transition TOFF to FLYING

include/eskf_odom_alg.h

@@ -51,6 +51,7 @@ class EskfOdomAlgorithm
     CEskf filter_;
 
   public:
+
     EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
    /**
@@ -153,6 +154,7 @@ class EskfOdomAlgorithm
     *   There are no specific inputs due to all are class variables.
     */
     void set_obs_phase(const bool& flying, const double& gnd_dist);
+    // void set_obs_phase(const bool& flying);
 
     /**
     * \brief Set IMU Readings
@@ -179,33 +181,15 @@ class EskfOdomAlgorithm
     void set_px4_reading(const double& t, const Eigen::Vector3d& val, const Eigen::Vector2d& flow);
 
     /**
-    * \brief Nominal State Propagation
+    * \brief Run next step
     *
-    * Nominal State propagation using IMU model.
+    * Run filter next step (either propagation or update and correction)
     *
-    * Inputs:
-    *   There are no specific inputs due to all are class variables.
-    */
-    void prop_nominal(void);
+    * Input/Ouput:
+    *   state:  Robot state (6D).
+    */  
+    bool run_next_step(Eigen::VectorXd& state);
 
-    /**
-    * \brief Observe estimation error and Update Nominal state
-    *
-    *  Observe estimation error and Update Nominal state.
-    *
-    * Inputs:
-    *   There are no specific inputs due to all are class variables.
-    */
-    void obs_error_and_up_nominal(void);
-
-    /**
-    * \brief Get nominal-state vector
-    *
-    * Get nominal-state vector. Function to prevent external modifications.
-    *
-    */
-    Eigen::VectorXd get_x_state(void);
-    
    /**
     * \brief Destructor
     *

include/eskf_odom_alg_node.h

@@ -58,6 +58,7 @@ using namespace std;
  *
  */
 class EskfOdomAlgNode: public algorithm_base::IriBaseAlgorithm<EskfOdomAlgorithm> {
+	
 private:
 
 	bool flying_; // Quadrotor landed or flying information.

src/eskf_odom_alg.cpp

@@ -39,6 +39,11 @@ void EskfOdomAlgorithm::set_obs_phase(const bool& flying, const double& gnd_dist
 	this->filter_.set_obs_phase(flying,gnd_dist);
 }
 
+// void EskfOdomAlgorithm::set_obs_phase(const bool& flying)
+// {
+//   this->filter_.set_obs_phase(flying);
+// }
+
 void EskfOdomAlgorithm::set_imu_reading(const double& t, const Eigen::Vector3d& a, const Eigen::Vector3d& w)
 {
 	this->filter_.set_imu_reading(t,a,w);
@@ -49,17 +54,7 @@ void EskfOdomAlgorithm::set_px4_reading(const double& t, const Eigen::Vector3d&
     this->filter_.set_px4_reading(t,val,flow);
 }
 
-void EskfOdomAlgorithm::prop_nominal(void)
-{
-	this->filter_.prop_nominal();
-}
-
-void EskfOdomAlgorithm::obs_error_and_up_nominal(void)
-{
-	this->filter_.px4_obs_error_and_up_nominal();
-}	
-
-VectorXd EskfOdomAlgorithm::get_x_state(void)
+bool EskfOdomAlgorithm::run_next_step(Eigen::VectorXd& state)
 {
-	return this->filter_.get_x_state();
+  return this->filter_.run_next_step(state);
 }
\ No newline at end of file

src/eskf_odom_alg_node.cpp

@@ -172,8 +172,7 @@ void EskfOdomAlgNode::read_and_set_ini_params(void)
 	this->alg_.lock();  // protect algorithm
 
 	// Set filter and sensors initial parameters
-	this->alg_.set_init_params(f_params, x_state, dx_state, imu_params,
-			px4_params);
+	this->alg_.set_init_params(f_params, x_state, dx_state, imu_params,	px4_params);
 
 #ifdef READ_FROM_FILE
 	this->public_node_handle_.param<string>("data_path", this->data_path_, "");
@@ -258,11 +257,6 @@ void EskfOdomAlgNode::mainNodeThread(void)
 
 	// [publish messages]
 
-	// Set Quadrotor State
-	this->alg_.lock();  // protect algorithm
-	this->alg_.set_obs_phase(this->flying_, this->gnd_dist_); // Set observation phase (Landed, toff, Flying or Landing)
-	this->alg_.unlock();
-
 #ifdef READ_FROM_FILE
 
 	ROS_DEBUG("IMU process");
@@ -334,66 +328,28 @@ void EskfOdomAlgNode::mainNodeThread(void)
 
 #ifndef READ_FROM_FILE
 
-	if (this->imu_queue_.size() > 1 && this->px4_queue_.size() > 0)
-	{
-
-		double t_imu = this->imu_queue_.front()->header.stamp.toSec() - this->t_ini_imu_;
-		double t_px4 = this->px4_queue_.front()->header.stamp.toSec() - this->t_ini_px4_;
-
-		if (t_imu < t_px4)
-		{
-			// Set IMU data
-			set_imu_reading(this->imu_queue_.front(), t_imu);
-			this->imu_queue_.pop();
-
-			// first imu reading not used due to it is needed for the first nominal state integration
-			if (this->is_first_imu_)
-			{
-				this->is_first_imu_ = false;
-				//std::cout << "First IMU!" << std::endl;
-
-				// release next px4 beacuse first imu is not used.
-				this->px4_queue_.pop();
-			}
-			else{
-
-				//std::cout << "Process IMU with time: " << t_imu << std::endl;
-
-				// Propagate Filter
-				this->alg_.lock();  // protect algorithm
-				this->alg_.prop_nominal(); // Propagate Nominal-State
-				this->alg_.unlock();
-			}
-		}
-		else
-		{
-			//std::cout << "Process PX4 with time: " << t_px4 << std::endl;
-
-			//Set PX4 data
-			set_px4_reading(this->px4_queue_.front(), t_px4);
-			this->px4_queue_.pop();
-
-			//Filter update
-			this->alg_.lock();  // protect algorithm
-			this->alg_.obs_error_and_up_nominal(); // Error observation and Nominal-State Update
-			this->alg_.unlock();
-		}
-	}
+	this->alg_.lock();  
+	// Set observation phase (Landed, toff, Flying or Landing)
+	this->alg_.set_obs_phase(this->flying_,this->gnd_dist_); 
+	this->alg_.unlock();
 
 #endif
 
-	// Get state and publish TF **********************
-	this->alg_.lock();  // protect algorithm
-	Eigen::VectorXd state = this->alg_.get_x_state(); // get state
+	Eigen::VectorXd state(19,1);
+	this->alg_.lock();
+	bool step_done = this->alg_.run_next_step(state);
 	this->alg_.unlock();
 
-	// Broadcast TF with state
-	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)); //TF:[qx,qy,qz,qw] Filter:[qw,qx,qy,qz]
-	transform.setRotation(q);
-	br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", "base_link"));
+	if (step_done)
+	{
+  	// Broadcast state with TF
+  	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)); //TF:[qx,qy,qz,qw] Filter:[qw,qx,qy,qz]
+  	transform.setRotation(q);
+  	br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", "base_link"));
+	};
 }
 
 #ifndef READ_FROM_FILE
@@ -420,10 +376,10 @@ void EskfOdomAlgNode::hl_status_mutex_exit(void)
 	pthread_mutex_unlock(&this->hl_status_mutex_);
 }
 
-void EskfOdomAlgNode::ll_status_callback(
-		const asctec_msgs::LLStatus::ConstPtr& msg)
+void EskfOdomAlgNode::ll_status_callback(const asctec_msgs::LLStatus::ConstPtr& msg)
 {
 	ROS_DEBUG("EskfOdomAlgNode::ll_status_callback: New Message Received");
+
 	this->ll_status_mutex_enter();
 	this->flying_ = msg->flying;
 	this->ll_status_mutex_exit();
@@ -439,8 +395,7 @@ void EskfOdomAlgNode::ll_status_mutex_exit(void)
 
 void EskfOdomAlgNode::px4_of_callback(const px_comm::OpticalFlow::ConstPtr& msg)
 {
-	//    ROS_DEBUG("EskfOdomAlgNode::px4_of_callback: New Message Received");
-
+	// ROS_DEBUG("EskfOdomAlgNode::px4_of_callback: New Message Received");
 	ros::Time stamp = msg->header.stamp;
 	double t = stamp.toSec();
 
@@ -450,7 +405,7 @@ void EskfOdomAlgNode::px4_of_callback(const px_comm::OpticalFlow::ConstPtr& msg)
 		this->is_first_px4_ = false;
 	}
 
-	this->px4_queue_.push(msg);
+	set_px4_reading(msg, t-this->t_ini_px4_);
 }
 void EskfOdomAlgNode::px4_of_mutex_enter(void)
 {
@@ -463,16 +418,18 @@ void EskfOdomAlgNode::px4_of_mutex_exit(void)
 
 void EskfOdomAlgNode::imu_callback(const sensor_msgs::Imu::ConstPtr& msg)
 {
+	// ROS_DEBUG("EskfOdomAlgNode::imu_callback: New Message Received");
 	ros::Time stamp = msg->header.stamp;
 	double t = stamp.toSec();
 
 	if (this->is_first_imu_)
 	{
 		this->t_ini_imu_ = t;
-		//this->is_first_imu_ = false;
+		this->is_first_imu_ = false;
 	}
 
-	this->imu_queue_.push(msg);
+	// this->imu_queue_.push(msg);
+	set_imu_reading(msg, t-this->t_ini_imu_);
 }
 void EskfOdomAlgNode::imu_mutex_enter(void)
 {
@@ -483,8 +440,7 @@ void EskfOdomAlgNode::imu_mutex_exit(void)
 	pthread_mutex_unlock(&this->imu_mutex_);
 }
 
-void EskfOdomAlgNode::set_imu_reading(const sensor_msgs::Imu::ConstPtr& msg,
-		const double& msg_time)
+void EskfOdomAlgNode::set_imu_reading(const sensor_msgs::Imu::ConstPtr& msg,const double& msg_time)
 {
 	// Get sensor values
 	this->imu_mutex_enter();
@@ -506,8 +462,7 @@ void EskfOdomAlgNode::set_imu_reading(const sensor_msgs::Imu::ConstPtr& msg,
 	this->alg_.unlock();
 }
 
-void EskfOdomAlgNode::set_px4_reading(const px_comm::OpticalFlow::ConstPtr& msg,
-		const double& msg_time)
+void EskfOdomAlgNode::set_px4_reading(const px_comm::OpticalFlow::ConstPtr& msg,const double& msg_time)
 {
 	// Get sensor values
 	this->px4_of_mutex_enter();