Commit 29a361a7 authored by adrianamor's avatar adrianamor

Debugging: Before eskf_predict. IMU Trans Mat OK.

parent 5fa20026
......@@ -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]
......
......@@ -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();
}
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment