implemented robot phases and px4 STD

include/eskf_odom_alg.h

@@ -31,8 +31,6 @@
 
 //include eskf_odom_alg main library
 
-const double PX4_MIN_HEIGHT = 0.31;
-
 /**
  * \brief IRI ROS Specific Driver Class
  *

launch/params.yaml

@@ -15,7 +15,9 @@ imu_wb_std: [0.0,0.0,0.0]
 imu_method: continuous #Sets the data time nature (continous or discrete).
 
 # PX4 STD values
-px4_std: [0.01,0.1,0.1]
+px4_std_landed: [0.1,0.001,0.001]
+px4_std_below30cm: [10.0,10.0,10.0]
+px4_std_30cmto5m: [0.01,0.15,0.15]
 
 # Initial Nominal-State vector (p,v,q,ab,wb,g)
 xstate0: [0.0,0.0,0.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,-9.803057]

src/eskf_odom_alg.cpp

@@ -36,38 +36,5 @@ void EskfOdomAlgorithm::print_ini_params()
 
 void EskfOdomAlgorithm::set_obs_phase(const bool& flying, const double& gnd_dist)
 {
-	Robot::kinton_phases phase;
-	this->filter_.get_robot_phase(phase);
-
-	switch (phase) {
-	case Robot::LANDED:
-		if (flying){
-			this->filter_.set_robot_phase(Robot::TOFF);
-			this->filter_.print_robot_phase();
-		}
-		break;
-	case Robot::TOFF:
-		if (gnd_dist > PX4_MIN_HEIGHT){
-			this->filter_.set_robot_phase(Robot::FLYING);
-			this->filter_.print_robot_phase();
-		}
-		break;
-	case Robot::FLYING:
-		if (gnd_dist < PX4_MIN_HEIGHT){
-			this->filter_.set_robot_phase(Robot::LANDING);
-			this->filter_.print_robot_phase();
-		}
-		break;
-	case Robot::LANDING:
-		if (!flying){
-			this->filter_.set_robot_phase(Robot::LANDED);
-			this->filter_.print_robot_phase();
-		}
-		else
-			if (gnd_dist > PX4_MIN_HEIGHT){
-		  		this->filter_.set_robot_phase(Robot::FLYING);
-		  		this->filter_.print_robot_phase();
-		  	}
-			break;
-	}
+	this->filter_.set_obs_phase(flying,gnd_dist);
 }
\ No newline at end of file

src/eskf_odom_alg_node.cpp

@@ -18,7 +18,7 @@ EskfOdomAlgNode::EskfOdomAlgNode(void) :
 
   // Initialize ROBOT phase
   this->flying_ = false;
-  this->gnd_dist_ = PX4_MIN_HEIGHT; // a little bit more than the minimum distance PX4 can detect (0.3m)
+  this->gnd_dist_ = 0; // a little bit more than the minimum distance PX4 can detect (0.3m)
 
   // [init publishers]
   
@@ -87,7 +87,10 @@ void EskfOdomAlgNode::read_and_set_ini_params(void)
 
   // PX4 STD values
   Sensor::px4_params px4_params;
-  px4_params.std = read_vec("px4_std",3);
+  px4_params.std_landed = read_vec("px4_std_landed",3);
+  px4_params.std_below30cm = read_vec("px4_std_below30cm",3);
+  px4_params.std_30cmto5m = read_vec("px4_std_30cmto5m",3);
+  px4_params.std = px4_params.std_landed; // Considering initially landed
 
   // Initialize Nominal-State vector
   VectorXd x_state = read_vec("xstate0",19);