From 2f3b1fd0c1c2007e3ea16a7305955333b877e30f Mon Sep 17 00:00:00 2001 From: Moju Zhao Date: Wed, 31 Jan 2024 02:09:05 +0900 Subject: [PATCH 1/2] [Estimation][IMU] refactor the index designation of KF based estimation result --- aerial_robot_estimation/src/sensor/imu.cpp | 37 ++++++++-------------- 1 file changed, 14 insertions(+), 23 deletions(-) diff --git a/aerial_robot_estimation/src/sensor/imu.cpp b/aerial_robot_estimation/src/sensor/imu.cpp index 9836c2da2..654bf3fe9 100644 --- a/aerial_robot_estimation/src/sensor/imu.cpp +++ b/aerial_robot_estimation/src/sensor/imu.cpp @@ -389,29 +389,20 @@ namespace sensor_plugin /* publish state date */ state_.header.stamp = imu_stamp_; - tf::Vector3 pos = estimator_->getPos(Frame::BASELINK, aerial_robot_estimation::EGOMOTION_ESTIMATE); - tf::Vector3 vel = estimator_->getVel(Frame::BASELINK, aerial_robot_estimation::EGOMOTION_ESTIMATE); - state_.states[0].state[0].x = pos.x(); - state_.states[1].state[0].x = pos.y(); - state_.states[2].state[0].x = pos.z(); - state_.states[0].state[0].y = vel.x(); - state_.states[1].state[0].y = vel.y(); - state_.states[2].state[0].y = vel.z(); - state_.states[0].state[0].z = acc_w_.x(); - state_.states[1].state[0].z = acc_w_.y(); - state_.states[2].state[0].z = acc_w_.z(); - pos = estimator_->getPos(Frame::BASELINK, aerial_robot_estimation::EXPERIMENT_ESTIMATE); - vel = estimator_->getVel(Frame::BASELINK, aerial_robot_estimation::EXPERIMENT_ESTIMATE); - state_.states[0].state[1].x = pos.x(); - state_.states[1].state[1].x = pos.y(); - state_.states[2].state[1].x = pos.z(); - state_.states[0].state[1].y = vel.x(); - state_.states[1].state[1].y = vel.y(); - state_.states[2].state[1].y = vel.z(); - state_.states[0].state[1].z = acc_w_.x(); - state_.states[1].state[1].z = acc_w_.y(); - state_.states[2].state[1].z = acc_w_.z(); - + for (int i = 0; i < 2; i++) + { + tf::Vector3 pos = estimator_->getPos(Frame::BASELINK, i); + tf::Vector3 vel = estimator_->getVel(Frame::BASELINK, i); + state_.states[0].state[i].x = pos.x(); + state_.states[1].state[i].x = pos.y(); + state_.states[2].state[i].x = pos.z(); + state_.states[0].state[i].y = vel.x(); + state_.states[1].state[i].y = vel.y(); + state_.states[2].state[i].y = vel.z(); + state_.states[0].state[i].z = acc_w_.at(i).x(); + state_.states[1].state[i].z = acc_w_.at(i).y(); + state_.states[2].state[i].z = acc_w_.at(i).z(); + } state_pub_.publish(state_); } prev_time = imu_stamp_; From 4ad039dce1c1f4262733739fcb55e86ef592adc8 Mon Sep 17 00:00:00 2001 From: Moju Zhao Date: Fri, 12 Apr 2024 12:08:11 +0900 Subject: [PATCH 2/2] [Estimation][IMU] fix the bug about wrong assigment for acceleration --- aerial_robot_estimation/src/sensor/imu.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/aerial_robot_estimation/src/sensor/imu.cpp b/aerial_robot_estimation/src/sensor/imu.cpp index 654bf3fe9..42cdf9e80 100644 --- a/aerial_robot_estimation/src/sensor/imu.cpp +++ b/aerial_robot_estimation/src/sensor/imu.cpp @@ -399,9 +399,9 @@ namespace sensor_plugin state_.states[0].state[i].y = vel.x(); state_.states[1].state[i].y = vel.y(); state_.states[2].state[i].y = vel.z(); - state_.states[0].state[i].z = acc_w_.at(i).x(); - state_.states[1].state[i].z = acc_w_.at(i).y(); - state_.states[2].state[i].z = acc_w_.at(i).z(); + state_.states[0].state[i].z = acc_w_.x(); + state_.states[1].state[i].z = acc_w_.y(); + state_.states[2].state[i].z = acc_w_.z(); } state_pub_.publish(state_); }