Hi,

I have designed a backstepping controller and verified that it worked well in Matlab. The goal of controller is to make the hovercraft follow the given circular trajectory in 2D. But the controller implemented in C++ does not work well in ROS. The hovercraft mostly rotates in gazebo simulation wheares it follows the circular trajectory in Matlab. I think I have checked every variables, time step, and etc. If there is anyone who has those experience, could you let me know something which I have to check to make my controller implemented in C++ work in ROS ?

#include <ros/ros.h>
#include <nav_msgs/Odometry.h>
#include <geometry_msgs/Wrench.h>
#include <algorithm>
#include <eigen3/Eigen/Core>
#include <eigen3/Eigen/LU>

//#include <LinearMath/btQuaternion.h> // Needed to convert rotation ...
//#include <LinearMath/btMatrix3x3.h>  // ... quaternion into Euler angles


class Controller
{
public:
  Controller();

private:
  void controllerCallback(const nav_msgs::Odometry::ConstPtr& controller);

  ros::NodeHandle nh_;
  ros::Publisher left_fan_force_pub_;
  ros::Publisher right_fan_force_pub_;
  ros::Subscriber odom_sub_;

  double goal_x;
  double goal_y;
  double old_time;
  double old_x;
  double old_y;
  Eigen::Vector2d old_alpha;
  bool init_odom_;
  double init_odom_time_;
  geometry_msgs::Wrench left_fan_force_;
  geometry_msgs::Wrench right_fan_force_;
};


Controller::Controller()
{
  init_odom_ = true;
  old_time = ros::Time::now().toSec();
  old_x = 0;
  old_y = 0;
  Eigen::Vector2d old_alpha(0, 0);
  goal_x = 5.0;
  goal_y = 0.0;
  psi = 0;
  left_fan_force_pub_ = nh_.advertise<geometry_msgs::Wrench>("left_fan_force", 1);
  right_fan_force_pub_ = nh_.advertise<geometry_msgs::Wrench>("right_fan_force", 1);
  odom_sub_ = nh_.subscribe<nav_msgs::Odometry>("base_pose_ground_truth", 10, &Controller::controllerCallback, this);
}

void Controller::controllerCallback(const nav_msgs::Odometry::ConstPtr& odom)
{
  if (init_odom_){
    init_odom_time_ = odom->header.stamp.toSec();
    init_odom_ = false;
  }
  double m = 15.67;
  double d = 0.2;
  double Izz = 0.952;
  double u_des = 0.2;
  double omega_des = 0.1;
  double time = odom->header.stamp.toSec() - init_odom_time_;
  double dt = time - old_time;
  double psi_des = omega_des*time;


//  std::cout << 2*sin(psi_des) << " " << -2*cos(psi_des) + 2 << std::endl;
//  std::cout << psi_des << std::endl;

  double q0 = odom->pose.pose.orientation.w;
  double q1 = odom->pose.pose.orientation.x;
  double q2 = odom->pose.pose.orientation.y;
  double q3 = odom->pose.pose.orientation.z;

  double vx = odom->twist.twist.linear.x;
  double vy = odom->twist.twist.linear.y;
  double omega = odom->twist.twist.angular.z;

  double psi = atan2(2*(q0*q3+q1*q2),1-2*(q2*q2+q3*q3));

//  std::cout << psi << std::endl;

  Eigen::Vector2d vel(vx, vy);
  Eigen::Matrix2d R; // This R is actaully the transposed R.
  R << cos(psi), sin(psi),
      -sin(psi), cos(psi); 

  Eigen::Vector2d delta(-0.05, 0);
  Eigen::Vector2d nu = R*vel;
  Eigen::Vector2d Pdot_des(u_des*cos(psi_des), u_des*sin(psi_des));
  Eigen::Vector2d P2dot_des(-u_des*omega_des*sin(psi_des),
                             u_des*omega_des*cos(psi_des));

  double dv = 0.45; //translational damping coeffcient ;
  double Ke = 4; // This is controller parameter
  Eigen::Matrix2d Dv;
  Dv << dv, 0, 0, dv;
  Eigen::Matrix2d Kphi;
  Kphi << 2, 0, 0, 2; // This is controller parameter
  Eigen::Matrix2d M;
  M << m, 0, 0, m;
  Eigen::Matrix2d Minv;
  Minv << 1/m, 0, 0, 1/m;

  Eigen::Vector2d P(odom->pose.pose.position.x, odom->pose.pose.position.y);
  Eigen::Vector2d Pdes(2*sin(psi_des), -2*cos(psi_des) + 2);
  Eigen::Vector2d e = R*(P - Pdes);
  Eigen::Matrix2d B;
  B << 1, -m*delta(1), 0, m*delta(0);

  Eigen::Vector2d z1 = nu - R*Pdot_des + Ke/m*e;
  Eigen::Vector2d h = -Dv*(delta + R*Pdot_des - Ke/m*e) - M*R*P2dot_des + Ke*z1
                      - pow(Ke, 2)/m*e;
  Eigen::Vector2d phi = z1 - delta;
  Eigen::Vector2d alpha = -B.inverse()*(Minv*e + h + Minv*Kphi*phi);
  double u1 = alpha(0);

//  std::cout << psi << std::endl;  
//  std::cout << Pdot_des(0) << " " << Pdot_des(1) << std::endl;  
//  std::cout << old_time << " " << time << "  " << psi << std::endl;

  Eigen::Matrix2d S;
  S << 0, -omega, omega, 0;
//  S << 0, omega, -omega, 0;
  Eigen::Vector2d e_dot = -S*e + nu - R*Pdot_des;
  Eigen::Vector2d h_dot = -Dv*(-S*R*Pdot_des + R*P2dot_des - Ke/m*(-S*e + nu - R*Pdot_des));

  Eigen::Matrix2d M_nu;
  M_nu <<  dv/m, -omega, omega, dv/m;
  Eigen::Vector2d M_u(u1/m, 0);
  Eigen::Vector2d nu_dot = -M_nu*nu + M_u;


/*
  Eigen::Vector2d P2dot((odom->twist.twist.linear.x - old_x)/dt,
                        (odom->twist.twist.linear.y - old_y)/dt);
  old_x = odom->twist.twist.linear.x;
  old_y = odom->twist.twist.linear.y;
*/
//  std::cout << old_x << " " << old_y << std::endl;

  Eigen::Vector2d phi_dot = nu_dot + S*R*Pdot_des - R*P2dot_des +
                            Ke/m*(-S*e + nu - R*Pdot_des);

  Eigen::Vector2d alpha_dot = -B.inverse()*(Minv*e_dot + h_dot + Minv*Kphi*phi_dot);

//  std::cout << P2dot(0) << " " << P2dot(1) << std::endl;

  double alpha2dot = alpha_dot(1);
  double alpha2 = alpha(1);
  Eigen::Vector2d Bb(-m*delta(1), m*delta(0));
  double dw = 0.41; //0.41;
  double Kz2 = 2; // This is controller parameter
  double z2 = omega - alpha(1);
  double u2 = -Bb.dot(M*phi) + dw*alpha2 + Izz*alpha2dot - Kz2*z2;

//  std::cout << u1 << " " << u2 << std::endl;

  double f1 = 0.5*(u1 - u2/d);
  double f2 = 0.5*(u1 + u2/d);

  Eigen::Vector2d force_int(f1, f2);
  Eigen::Vector2d force_SP = R*force_int;

if (time < 5){
  std::cout << u1 <<" "<< u2 <<" "<< alpha(1) <<" "<< alpha2dot << std::endl;  
//  std::cout << nu(1) <<" "<< nu_dot(0) <<" "<< nu_dot(1) << std::endl;
//  std::cout << f1 << " " << f2 << std::endl;
//  std::cout << P(0) << " " << P(1) << std::endl;
}

  left_fan_force_.force.x = force_SP(0);
  right_fan_force_.force.x = force_SP(1);

  old_time = time;
}


int main(int argc, char** argv)
{
  ros::init(argc, argv, "hover_controller");
  Controller hover_controller;
  ros::spin();
}