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

remove some useless code

This commit is contained in:
libing64 2016-02-29 19:39:41 +08:00
parent f248e73cc2
commit 0019e08d2d
2 changed files with 12 additions and 36 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
src/att_ekf.cpp
View File

@ -80,7 +80,6 @@ void Att_ekf::update_magnetic(Vector3d& mag, double t)
MatrixXd K = P.inverse()*H.transpose()*(H*P.inverse()*H.transpose() + R_mag).inverse(); MatrixXd K = P.inverse()*H.transpose()*(H*P.inverse()*H.transpose() + R_mag).inverse();
MatrixXd I = MatrixXd::Identity(12, 12); MatrixXd I = MatrixXd::Identity(12, 12);
//cout << "mag K: " << K << endl;
x = x + K*(z - H*x); x = x + K*(z - H*x);
P = (I- K*H)*P.inverse(); P = (I- K*H)*P.inverse();
//cout << "update mag: " << x.transpose() << endl; //cout << "update mag: " << x.transpose() << endl;
@ -112,7 +111,7 @@ void Att_ekf::update_imu(Vector3d &acc, Vector3d & gyro, double t)
MatrixXd K = P.inverse()*H.transpose()*(H*P.inverse()*H.transpose() + R_imu).inverse(); MatrixXd K = P.inverse()*H.transpose()*(H*P.inverse()*H.transpose() + R_imu).inverse();
MatrixXd I = MatrixXd::Identity(12, 12); MatrixXd I = MatrixXd::Identity(12, 12);
//cout << "imu K: " << K << endl;
x = x + K*(z - H*x); x = x + K*(z - H*x);
P = (I- K*H)*P.inverse(); P = (I- K*H)*P.inverse();
//cout << "update imu: " << x.transpose() << endl; //cout << "update imu: " << x.transpose() << endl;

45
src/att_ekf_node.cpp
View File

@ -21,11 +21,8 @@ deque<pair<double, sensor_msgs::Imu> >imu_q;
ros::Subscriber imu_bias_sub; ros::Subscriber imu_bias_sub;
bool imu_bias_initialized = false; bool imu_bias_initialized = false;
Vector3d gyro_bias; Vector3d gyro_bias, acc_bias;
Vector3d acc_bias;
Quaterniond q_gt;
double t_gt;
ros::Publisher pose_pub, pose_gt_pub; ros::Publisher pose_pub, pose_gt_pub;
void magCallback(const geometry_msgs::Vector3StampedConstPtr& msg) void magCallback(const geometry_msgs::Vector3StampedConstPtr& msg)
@ -33,11 +30,6 @@ void magCallback(const geometry_msgs::Vector3StampedConstPtr& msg)
geometry_msgs::Vector3Stamped mag_msg = *msg; geometry_msgs::Vector3Stamped mag_msg = *msg;
double t = msg->header.stamp.toSec(); double t = msg->header.stamp.toSec();
mag_q.push_back(make_pair(t, mag_msg)); mag_q.push_back(make_pair(t, mag_msg));
// Vector3d mag;
// mag(0) = msg->vector.x;
// mag(1) = msg->vector.y;
// mag(2) = msg->vector.z;
//cout << "mag: " << mag.transpose() << endl;
} }
@ -47,22 +39,15 @@ void imuCallback(const sensor_msgs::ImuConstPtr & msg)
sensor_msgs::Imu imu_msg = *msg; sensor_msgs::Imu imu_msg = *msg;
imu_q.push_back(make_pair(t, imu_msg)); imu_q.push_back(make_pair(t, imu_msg));
// Quaterniond q; //publish the groundtruth of the pose
q_gt.w() = msg->orientation.w; geometry_msgs::PoseStamped pose;
q_gt.x() = msg->orientation.x; pose.header.stamp = msg->header.stamp;
q_gt.y() = msg->orientation.y; pose.header.frame_id = "/base_footprint";
q_gt.z() = msg->orientation.z; pose.pose.orientation.w = msg->orientation.w;
t_gt = msg->header.stamp.toSec(); pose.pose.orientation.x = msg->orientation.x;
pose.pose.orientation.y = msg->orientation.y;
geometry_msgs::PoseStamped pose_gt; pose.pose.orientation.z = msg->orientation.z;
pose_gt.header.stamp = ros::Time(t_gt); pose_gt_pub.publish(pose);
pose_gt.header.frame_id = "/base_footprint";
pose_gt.pose.orientation.w = q_gt.w();
pose_gt.pose.orientation.x = q_gt.x();
pose_gt.pose.orientation.y = q_gt.y();
pose_gt.pose.orientation.z = q_gt.z();
pose_gt_pub.publish(pose_gt);
//cout << "q_gt: " << q_gt.w() << " " << q_gt.vec().transpose() << endl;
} }
void imuBiasCallback(const sensor_msgs::ImuConstPtr & msg) void imuBiasCallback(const sensor_msgs::ImuConstPtr & msg)
@ -140,17 +125,9 @@ int main(int argc, char **argv)
} }
if(imu_q.size() == 0 || mag_q.size() == 0) break; if(imu_q.size() == 0 || mag_q.size() == 0) break;
} }
// cout << "t: " << att_ekf.get_time() << endl;
// Quaterniond q = mat2quaternion(att_ekf.get_rotation_matrix());
// cout << "q: " << q.w() << " " << q.vec().transpose() << endl;
// cout << "q_gt: " << q_gt.w() << " " << q_gt.vec().transpose() << endl;
//Quaterniond dq = q.inverse()*q_gt;
//cout << "dq: " << dq.w() << " " << dq.vec().transpose() << endl;
loop_rate.sleep(); loop_rate.sleep();
} }
ros::spin();
return 0; return 0;
} }