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();
}
