p71924506
/
att_ekf
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

Compare commits

merge into: p71924506:master
Branches Tags
p71924506:master
p71924506:experiment
...
pull from: p71924506:experiment
Branches Tags
p71924506:experiment
p71924506:master

1 Commits
master ... experiment

Author SHA1 Message Date
libing64 c4c359a593 test att_ekf with real data 2016-05-28 20:24:48 +08:00
2 changed files with 22 additions and 8 deletions
Show Stats Expand all files Collapse all files

11
launch/att_ekf_exp.launch Normal file
View File

@ -0,0 +1,11 @@
<launch>
<node pkg="att_ekf" type="att_ekf" name="att_ekf" output="screen">
<remap from="/imu" to="/imu_3dm_gx4/imu"/>
<remap from="/magnetic_field" to="/imu_3dm_gx4/magnetic_field" />
<!-- <remap from="/imu_bias" to="/raw_imu/bias" /> -->
<!-- <remap from="/pose_gt" to="/ground_truth_to_tf/pose" /> -->
</node>
<!-- Start rviz visualization with preset config -->
<node pkg="rviz" type="rviz" name="rviz" args="-d $(find att_ekf)/rviz_cfg/att_estimation.rviz"/>
</launch>

19
src/att_ekf_node.cpp
View File

@ -15,7 +15,7 @@ using namespace std;
using namespace Eigen;
Att_ekf att_ekf;
deque<pair<double, geometry_msgs::Vector3Stamped> > mag_q;
deque<pair<double, Vector3d> > mag_q;
deque<pair<double, sensor_msgs::Imu> >imu_q;
@ -27,11 +27,14 @@ Vector3d currentPosition;
ros::Publisher pose_pub;
void magCallback(const geometry_msgs::Vector3StampedConstPtr& msg)
void magCallback(const sensor_msgs::MagneticFieldConstPtr& msg)
{
geometry_msgs::Vector3Stamped mag_msg = *msg;
Vector3d mag;
mag(0) = msg->magnetic_field.x;
mag(1) = msg->magnetic_field.y;
mag(2) = msg->magnetic_field.z;
double t = msg->header.stamp.toSec();
mag_q.push_back(make_pair(t, mag_msg));
mag_q.push_back(make_pair(t, mag));
}
@ -118,10 +121,10 @@ int main(int argc, char **argv)
imu_q.pop_front();
}else
{
Vector3d mag;
mag(0) = mag_q.front().second.vector.x;
mag(1) = mag_q.front().second.vector.y;
mag(2) = mag_q.front().second.vector.z;
Vector3d mag = mag_q.front().second;
// mag(0) = mag_q.front().second.vector.x;
// mag(1) = mag_q.front().second.vector.y;
// mag(2) = mag_q.front().second.vector.z;
double t = mag_q.front().first;
att_ekf.update_magnetic(mag, t);
publish_pose();