void compute_mean(SE3Type & mean_pose, Vector6 & mean_velocity) { mean_velocity.setZero(); for (int i = 0; i < 25; i++) { mean_velocity += sigma_velocity[i]; } mean_velocity /= 25.0f; mean_pose = sigma_pose[0]; Vector6 delta; const static int max_iterations = 1000; int iterations = 0; do { delta.setZero(); for (int i = 0; i < 25; i++) { delta += SE3Type::log(mean_pose.inverse() * sigma_pose[i]); } delta /= 25.0f; mean_pose *= SE3Type::exp(delta); iterations++; } while (delta.array().abs().maxCoeff() > Sophus::SophusConstants<_Scalar>::epsilon() && iterations < max_iterations); }
Vector3 computeDesiredForce(const SE3Type & pose, const Vector3 & linear_velocity, double delta_time) { Vector3 desired_position(0,0,1); Vector3 desired_velocity(0,0,0); // Vector3 kp(1,1,1) ; // Vector3 kd(1,1,1); // Vector3 ki(0.0015,0.0015,0.0015) ; //for ukf after redirecting to log Vector3 kp(4,4,4) ; Vector3 kd(3,3,3); Vector3 ki(0.002,0.002,0.002) ; Vector3 current_position=pose.translation(); std::cout << "\nCurrent Position" << current_position ; Vector3 proportiona_val= kp.cwiseProduct(desired_position-current_position); Vector3 differential_val=kd.cwiseProduct(desired_velocity-linear_velocity); accumulated_error = accumulated_error+(desired_position-current_position) ; Vector3 integral_value=ki.cwiseProduct(accumulated_error); Vector3 desired_force=proportiona_val+ differential_val+integral_value; std::cout << "\ndesired_force is" << desired_force ; return desired_force; }
mav_msgs::CommandAttitudeThrust computeCommandFromForce( const Vector3 & control_force, const SE3Type & pose, double delta_time) { command.header.stamp=ros::Time::now(); command.header.frame_id=1 ; command.yaw_rate=0 ; Eigen::Quaterniond orientation=pose.so3().unit_quaternion(); double x=orientation.x(); double y=orientation.y(); double z=orientation.z(); double w=orientation.w(); double current_yaw_angle = atan2(2*x*y + 2*w*z, w*w + x*x - y*y - z*z); command.roll=(control_force[0]*sin(current_yaw_angle)- control_force[1]*cos(current_yaw_angle))/g; command.pitch=(control_force[0]*cos(current_yaw_angle) + control_force[1]*sin(current_yaw_angle))/g ; command.thrust= control_force[2] + (m*g); std::cout << "\nThrust applied is" << command.thrust ; return command ; }
void compute_mean_and_covariance() { compute_mean(pose, velocity); covariance.setZero(); for (int i = 0; i < 25; i++) { Vector12 eps; eps.template head<6>() = SE3Type::log( pose.inverse() * sigma_pose[i]); eps.template tail<6>() = sigma_velocity[i] - velocity; covariance += eps * eps.transpose(); } covariance /= 2; }
void pose1Callback( const geometry_msgs::PoseWithCovarianceStampedConstPtr& msg) { Eigen::Quaterniond orientation; Eigen::Vector3d position; tf::pointMsgToEigen(msg->pose.pose.position, position); tf::quaternionMsgToEigen(msg->pose.pose.orientation, orientation); SE3Type pose(orientation, position); SE3Type transformed_pose = T_imu_cam.inverse() * pose; std::cout<<"Pose1 Position"<<"\n"<<transformed_pose.translation(); Matrix6 noise(&msg->pose.covariance[0]); std::cout<<"Noise "<<"\n"<<noise<<std::endl; ukf->measurePose( pose,noise); }
void groundTruthPoseCallback( const geometry_msgs::PoseWithCovarianceStampedConstPtr& msg) { Eigen::Quaterniond orientation; Eigen::Vector3d position; tf::pointMsgToEigen(msg->pose.pose.position, position); tf::quaternionMsgToEigen(msg->pose.pose.orientation, orientation); SE3Type pose(orientation.cast<_Scalar>(), position.cast<_Scalar>()); ground_truth_linear_velocity = (pose.translation() - ground_truth_pose.translation()) / (msg->header.stamp.toSec() - ground_truth_time); ground_truth_pose = pose; ground_truth_time = msg->header.stamp.toSec(); }