//for (int i = 0; i < fileParse->frame_count; i++)
  //{
  //  bool left_foot_contact = NCGetContactState(index, i, 6);
  //  bool right_foot_contact = NCGetContactState(index, i, 3);
  //  cout << right_foot_contact << endl;
  //  if (right_foot_contact == true)
  //  {
  //    float * getx = NCGetX(index, i);
  //    float * get_posi_in_ned = NCGetPosiInNED(index, i, 6);
  //    cout << getx[6 * 6 + 0] << "\t" << getx[6 * 6 + 1] << "\t" << getx[6 * 6 + 2] << endl;
  //    cout << getx[3 * 6 + 0] << "\t" << getx[3 * 6 + 1] << "\t" << getx[3 * 6 + 2] << endl;
  //    //cout << get_posi_in_ned[0] << "\t" << get_posi_in_ned[1] << "\t" << get_posi_in_ned[2] << endl;
  //  }

  //}

 //method1
  //Eigen::Quaternionf q_s = { arm_q_start[0], arm_q_start[1], arm_q_start[2], arm_q_start[3] };
  //Eigen::Quaternionf q_e = { arm_q_end[0], arm_q_end[1], arm_q_end[2], arm_q_end[3] };
  //Eigen::Quaternionf q_res = q_s * q_e;
  //Eigen::Vector3f euler_angles = q_res.matrix().eulerAngles(1, 0, 2); // YXZ
  //Eigen::Vector3f euler_angels_new = euler_angles * 180.0f / 3.14159265;
  //cout << euler_angels_new << endl;
  //method2
 /* Eigen::Quaternionf q_e = { arm_q_end[0], arm_q_end[1], arm_q_end[2], arm_q_end[3] };
  Eigen::Vector3f ned_vector = { 0,0,1 };
  cout << q_e.matrix().col(0) << endl;
  float a = acos(q_e.matrix().col(0).dot(ned_vector)) * 180.0f / 3.14159265;
  cout << a << endl;*/

 Eigen::Matrix3f aa(3, 3),BB(3,6), cc(3,6);
   aa.setZero();
   BB.setZero();
   cc.setZero();
    aa = q.matrix();
    BB = aa.inverse();
    cc = q.matrix().inverse() * BB;
   cout << cc << endl;