diff --git a/CHANGELOG.rst b/CHANGELOG.rst index 0d9c100..d5f40cf 100644 --- a/CHANGELOG.rst +++ b/CHANGELOG.rst @@ -2,6 +2,14 @@ Changelog for package teamsannio_med_control ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.1.4 (2018-06-11) +------------------ +* added the Kalman filter +* added plots in the launch file to monitor the position and linear velocity errors between the Kalman filter output and the odometry ground truth values. In order to develop such functionality, the teamsannio messages have been created. +* A yaml file needed to set up the Kalman filter has been created. Such file contains the tuning matrix of the filter and the standard deviations that characterize the odometry virtual sensor. +* The Kalman filter works with the noisy attitude (standard deviation 0.017). This function considers the nonideality of the attitude filter onboard the quadrotor. +* Contributors: Pasquale Oppido, Giuseppe Silano, Luigi Iannelli + 0.1.3 (2018-05-25) ----------- * added the data storage section. Such section has been inserted in the position controller node and allows to storage, in defined csv files, the aircraft and controller state. @@ -10,19 +18,19 @@ Changelog for package teamsannio_med_control * Contributors: Giuseppe Silano, Luigi Iannelli 0.1.2 (2018-05-15) ------------ +------------------ * added the basic.world file inside the repository * the launch files have beed modified ad hoc * the ieration numbers has been moved from 1000 to 50 in order to speed up the simulation * Contributors: Giuseppe Silano, Luigi Iannelli 0.1.1 (2018-05-13) ------------ +------------------ * fixed issues in the control law development * Contributors: Giuseppe Silano, Pasquale Oppido, Luigi Iannelli 0.1.0 (2018-04-30) ------------ +------------------ * initial Ubuntu package release * Contributors: Giuseppe Silano, Pasquale Oppido, Luigi Iannelli diff --git a/CMakeLists.txt b/CMakeLists.txt index c55ac5d..ac6258f 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -61,6 +61,7 @@ install(TARGETS position_controller install( DIRECTORY include/${PROJECT_NAME}/ + DIRECTORY include/teamsannio_msgs/ DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} FILES_MATCHING PATTERN "*.h" ) diff --git a/include/teamsannio_med_control/extendedKalmanFilter.h b/include/teamsannio_med_control/extendedKalmanFilter.h index d651922..7caaf08 100644 --- a/include/teamsannio_med_control/extendedKalmanFilter.h +++ b/include/teamsannio_med_control/extendedKalmanFilter.h @@ -19,32 +19,58 @@ #ifndef _ESTIMATOR_EXTENDED_KALMAN_FILTER_H_ #define _ESTIMATOR_EXTENDED_KALMAN_FILTER_H_ -#include #include "teamsannio_med_control/transform_datatypes.h" #include "teamsannio_med_control/Matrix3x3.h" #include "teamsannio_med_control/Quaternion.h" + #include #include +#include +#include + +#include +#include #include "stabilizer_types.h" +#include "filter_parameters.h" #include "common.h" - +#include "parameters_ros.h" namespace teamsannio_med_control { class ExtendedKalmanFilter { public: - ExtendedKalmanFilter(); - ~ExtendedKalmanFilter(); + + ExtendedKalmanFilter(); + ~ExtendedKalmanFilter(); - void Estimator(state_t *state_, EigenOdometry* odometry_); + void Estimator(state_t *state_, EigenOdometry* odometry_, nav_msgs::Odometry* odometry_filtered); + void SetThrustCommand(double u_T); + void SetVehicleParameters(double m, double g); + void SetFilterParameters(FilterParameters *filter_parameters_); private: - EigenOdometry odometry_private_; + EigenOdometry odometry_private_; + + //Filter Vectors + Eigen::VectorXf Xp_, Xe_, Hatx_; + Eigen::MatrixXf P_, Pe_; + Eigen::MatrixXf Rp_private_, Qp_private_; + + Eigen::MatrixXf A_private_, Qp_std_, Rp_std_, Hp_; + + std::normal_distribution distribution_; + + //Vehicle parameters + double m_private_, g_private_; + double u_T_private_; - void Quaternion2Euler(double* roll, double* pitch, double* yaw) const; - void SetOdometry(const EigenOdometry& odometry); + void Quaternion2Euler(double* roll, double* pitch, double* yaw) const; + void AttitudeAddingNoise(double *phin, double *thetan, double* psin, double phi, double theta, double psi); + void SetOdometry(const EigenOdometry& odometry); + void Correct(); + void Predict(); }; diff --git a/include/teamsannio_med_control/filter_parameters.h b/include/teamsannio_med_control/filter_parameters.h new file mode 100644 index 0000000..008d494 --- /dev/null +++ b/include/teamsannio_med_control/filter_parameters.h @@ -0,0 +1,76 @@ +/* + * Copyright 2018 Giuseppe Silano, University of Sannio in Benevento, Italy + * Copyright 2018 Pasquale Oppido, University of Sannio in Benevento, Italy + * Copyright 2018 Luigi Iannelli, University of Sannio in Benevento, Italy + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef _FILTER_PARAMTERS_H_ +#define _FILTER_PARAMTERS_H_ + +#include +#include + +namespace teamsannio_med_control { + +static constexpr double DefaultDevX = 0.01; +static constexpr double DefaultDevY = 0.01; +static constexpr double DefaultDevZ = 0.01; + +static constexpr double DefaultDevVX = 0.01; +static constexpr double DefaultDevVY = 0.01; +static constexpr double DefaultDevVZ = 0.01; + +static constexpr double DefaultQpX = 1e-6; +static constexpr double DefaultQpY = 1e-6; +static constexpr double DefaultQpZ = 1e-6; + +static constexpr double DefaultQpVX = 1e-6; +static constexpr double DefaultQpVY = 1e-6; +static constexpr double DefaultQpVZ = 1e-6; + +class FilterParameters { + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + FilterParameters() + : dev_x_(DefaultDevX), + dev_y_(DefaultDevY), + dev_z_(DefaultDevZ), + dev_vx_(DefaultDevVX), + dev_vy_(DefaultDevVY), + dev_vz_(DefaultDevVZ), + Qp_x_(DefaultQpX), + Qp_y_(DefaultQpY), + Qp_z_(DefaultQpZ), + Qp_vx_(DefaultQpVX), + Qp_vy_(DefaultQpVY), + Qp_vz_(DefaultQpVZ), + Rp_(Eigen::MatrixXf::Zero(6,6)), + Qp_(Eigen::MatrixXf::Identity(6,6)){ + } + + Eigen::MatrixXf Rp_, Qp_; + + double dev_x_, Qp_x_; + double dev_y_, Qp_y_; + double dev_z_, Qp_z_; + + double dev_vx_, Qp_vx_; + double dev_vy_, Qp_vy_; + double dev_vz_, Qp_vz_; +}; + +} + +#endif // _FILTER_PARAMTERS_H_ diff --git a/include/teamsannio_med_control/parameters.h b/include/teamsannio_med_control/parameters.h index c7791ae..42a857b 100644 --- a/include/teamsannio_med_control/parameters.h +++ b/include/teamsannio_med_control/parameters.h @@ -1,3 +1,21 @@ +/* + * Copyright 2018 Giuseppe Silano, University of Sannio in Benevento, Italy + * Copyright 2018 Pasquale Oppido, University of Sannio in Benevento, Italy + * Copyright 2018 Luigi Iannelli, University of Sannio in Benevento, Italy + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + #ifndef INCLUDE_BEBOP_CONTROL_PARAMETERS_H_ #define INCLUDE_BEBOP_CONTROL_PARAMETERS_H_ diff --git a/include/teamsannio_med_control/position_controller.h b/include/teamsannio_med_control/position_controller.h index d14a40e..11756db 100644 --- a/include/teamsannio_med_control/position_controller.h +++ b/include/teamsannio_med_control/position_controller.h @@ -28,6 +28,7 @@ #include #include "extendedKalmanFilter.h" +#include "filter_parameters.h" #include "stabilizer_types.h" #include "parameters.h" #include "common.h" @@ -36,8 +37,6 @@ using namespace std; - - namespace teamsannio_med_control { // Default values for the position controller of the Bebop. XYController [x,y], Roll Control [phi], @@ -56,8 +55,6 @@ static const double MuDefaultRollController = 0.09; static const double MuDefaultPitchController = 0.26; static const double MuDefaultYawRateController = 0.04; - - class PositionControllerParameters { public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW @@ -99,10 +96,13 @@ class PositionControllerParameters { void SetTrajectoryPoint(const mav_msgs::EigenTrajectoryPoint& command_trajectory); void SetControllerGains(); void SetVehicleParameters(); + void SetFilterParameters(); + void GetOdometry(nav_msgs::Odometry* odometry_filtered); PositionControllerParameters controller_parameters_; ExtendedKalmanFilter extended_kalman_filter_bebop_; VehicleParameters vehicle_parameters_; + FilterParameters filter_parameters_; EIGEN_MAKE_ALIGNED_OPERATOR_NEW private: @@ -179,11 +179,15 @@ class PositionControllerParameters { void CallbackPosition(const ros::TimerEvent& event); void CallbackSaveData(const ros::TimerEvent& event); + nav_msgs::Odometry odometry_filtered_private_; + state_t state_; + control_t control_; mav_msgs::EigenTrajectoryPoint command_trajectory_; EigenOdometry odometry_; void SetOdometryEstimated(); + void Quaternion2Euler(double* roll, double* pitch, double* yaw) const; void AttitudeController(double* u_phi, double* u_theta, double* u_psi); void AngularVelocityErrors(double* dot_e_phi_, double* dot_e_theta_, double* dot_e_psi_); void AttitudeErrors(double* e_phi_, double* e_theta_, double* e_psi_); diff --git a/include/teamsannio_med_control/stabilizer_types.h b/include/teamsannio_med_control/stabilizer_types.h index 58e46d3..c2ece46 100644 --- a/include/teamsannio_med_control/stabilizer_types.h +++ b/include/teamsannio_med_control/stabilizer_types.h @@ -125,9 +125,9 @@ typedef struct state_s { } state_t; typedef struct control_s { - int16_t roll; - int16_t pitch; - int16_t yaw; + double roll; + double pitch; + double yawRate; double thrust; } control_t; diff --git a/include/teamsannio_msgs/default_topics.h b/include/teamsannio_msgs/default_topics.h new file mode 100644 index 0000000..015a6f6 --- /dev/null +++ b/include/teamsannio_msgs/default_topics.h @@ -0,0 +1,33 @@ +/* + * Copyright 2018 Giuseppe Silano, University of Sannio in Benevento, Italy + * Copyright 2018 Pasquale Oppido, University of Sannio in Benevento, Italy + * Copyright 2018 Luigi Iannelli, University of Sannio in Benevento, Italy + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef KALMAN_FILTER_DEFAULT_TOPICS_H_ +#define KALMAN_FILTER_DEFAULT_TOPICS_H_ + +namespace teamsannio_msgs { + namespace default_topics { + + static constexpr char FILTERED_OUTPUT[] = "filteredOutput"; + static constexpr char STATE_ERRORS[] = "stateErrors"; + + static constexpr char ODOMETRY_GT[] = "odometry_gt"; + + + } // end namespace default_topics +} // end namespace teamsannio_msgs + +#endif /* KALMAN_FILTER_DEFAULT_TOPICS_H_ */ diff --git a/launch/task1_world.launch b/launch/task1_world.launch index b9da83c..43a12fa 100644 --- a/launch/task1_world.launch +++ b/launch/task1_world.launch @@ -36,9 +36,13 @@ + + + + @@ -50,4 +54,11 @@ + + + + + + + diff --git a/package.xml b/package.xml index f070ff0..d26240c 100644 --- a/package.xml +++ b/package.xml @@ -1,7 +1,7 @@ teamsannio_med_control - 1.0.0 + 0.1.4 This package provides the Bebop control algorithm. Giuseppe Silano @@ -14,6 +14,9 @@ Apache + https://github.com/gsilano/teamsannio_med_control + https://github.com/gsilano/teamsannio_med_control/issues + catkin diff --git a/resource/EKF_matrix.yaml b/resource/EKF_matrix.yaml new file mode 100644 index 0000000..ca62474 --- /dev/null +++ b/resource/EKF_matrix.yaml @@ -0,0 +1,10 @@ +# Tuning matrix of the Extended Kalman Filter (min 1e-5) +Qp: {aa: 0.00015, bb: 0.00015, cc: 0.00015, dd: 0.00015, ee: 0.00015, ff: 0.00015} +# Standard deviation of noise on positions and linear velocities +dev_x: 0.01 +dev_y: 0.01 +dev_z: 0.01 +dev_vx: 0.01 +dev_vy: 0.01 +dev_vz: 0.01 + diff --git a/src/library/extendedKalmanFilter.cpp b/src/library/extendedKalmanFilter.cpp index 9e4aa62..4856731 100644 --- a/src/library/extendedKalmanFilter.cpp +++ b/src/library/extendedKalmanFilter.cpp @@ -25,15 +25,46 @@ #include #include +#include #include #include #include +#include + +#define TsP 10e-3 /* Position control sampling time */ namespace teamsannio_med_control { -ExtendedKalmanFilter::ExtendedKalmanFilter() {} +ExtendedKalmanFilter::ExtendedKalmanFilter() + :Xp_(Eigen::VectorXf::Zero(6)), + Xe_(Eigen::VectorXf::Zero(6)), + P_(Eigen::MatrixXf::Zero(6,6)), + Pe_(Eigen::MatrixXf::Zero(6,6)), + Hatx_(Eigen::VectorXf::Zero(6)), + u_T_private_(0), + m_private_(0), + g_private_(0), + A_private_(Eigen::MatrixXf::Zero(6,6)), + Hp_(Eigen::MatrixXf::Identity(6,6)), + Rp_private_(Eigen::MatrixXf::Zero(6,6)), + Qp_private_(Eigen::MatrixXf::Identity(6,6)), + Qp_std_(Eigen::MatrixXf::Zero(6,6)), + Rp_std_(Eigen::MatrixXf::Zero(6,6)){ + + + A_private_ << 1, 0, 0, TsP, 0, 0, + 0, 1, 0, 0, TsP, 0, + 0, 0, 1, 0, 0, TsP, + 0, 0, 0, 1, 0, 0, + 0, 0, 0, 0, 1, 0, + 0, 0, 0, 0, 0, 1; + + double mean = 0, std = 0.017; + std::normal_distribution distribution_(mean, std); + +} ExtendedKalmanFilter::~ExtendedKalmanFilter() {} @@ -42,31 +73,44 @@ void ExtendedKalmanFilter::SetOdometry(const EigenOdometry& odometry) { odometry_private_ = odometry; } -void ExtendedKalmanFilter::Estimator(state_t *state_, EigenOdometry* odometry_){ +void ExtendedKalmanFilter::SetFilterParameters(FilterParameters *filter_parameters_){ + + Rp_private_ = filter_parameters_->Rp_; + Qp_private_ = filter_parameters_->Qp_; + + Qp_std_ = Qp_private_.transpose()*Qp_private_; + + Rp_std_ = Rp_private_.transpose()*Rp_private_; + +} + +void ExtendedKalmanFilter::Estimator(state_t *state_, EigenOdometry* odometry_, nav_msgs::Odometry* odometry_filtered){ assert(state_); assert(odometry_); SetOdometry(*odometry_); - state_->position.x = odometry_private_.position[0]; - state_->position.y = odometry_private_.position[1]; - state_->position.z = odometry_private_.position[2]; - - state_->linearVelocity.x = odometry_private_.velocity[0]; - state_->linearVelocity.y = odometry_private_.velocity[1]; - state_->linearVelocity.z = odometry_private_.velocity[2]; - - double roll, pitch, yaw; - Quaternion2Euler(&roll, &pitch, &yaw); - - state_->attitude.roll = roll; - state_->attitude.pitch = pitch; - state_->attitude.yaw = yaw; - - state_->angularVelocity.x = odometry_private_.angular_velocity[0]; - state_->angularVelocity.y = odometry_private_.angular_velocity[1]; - state_->angularVelocity.z = odometry_private_.angular_velocity[2]; - + Predict(); + Correct(); + + state_->position.x = Xe_(0); + state_->position.y = Xe_(1); + state_->position.z = Xe_(2); + + state_->linearVelocity.x = Xe_(3); + state_->linearVelocity.x = Xe_(4); + state_->linearVelocity.x = Xe_(5); + + Hatx_ = Xe_; + + *odometry_filtered; + odometry_filtered->pose.pose.position.x = Xe_(0); + odometry_filtered->pose.pose.position.y = Xe_(1); + odometry_filtered->pose.pose.position.z = Xe_(2); + odometry_filtered->twist.twist.linear.x = Xe_(3); + odometry_filtered->twist.twist.linear.y = Xe_(4); + odometry_filtered->twist.twist.linear.z = Xe_(5); + } void ExtendedKalmanFilter::Quaternion2Euler(double* roll, double* pitch, double* yaw) const { @@ -86,4 +130,107 @@ void ExtendedKalmanFilter::Quaternion2Euler(double* roll, double* pitch, double* } + +void ExtendedKalmanFilter::SetThrustCommand(double u_T){ + + u_T_private_ = u_T; + +} + +void ExtendedKalmanFilter::SetVehicleParameters(double m, double g){ + + m_private_ = m; + g_private_ = g; + +} + +void ExtendedKalmanFilter::Predict(){ + + double phi, theta, psi; + Quaternion2Euler(&phi, &theta, &psi); + + double phin, thetan, psin; + AttitudeAddingNoise(&phin, &thetan, &psin, phi, theta, psi); + + double x, y, z, dx, dy, dz; + x = Hatx_(0); + y = Hatx_(1); + z = Hatx_(2); + + dx = Hatx_(3); + dy = Hatx_(4); + dz = Hatx_(5); + + double dx_ENU, dy_ENU, dz_ENU; + + dx_ENU = (cos(thetan) * cos(psin) * dx) + + ( ( (sin(phin) * sin(thetan) * cos(psin) ) - ( cos(phin) * sin(psin) ) ) * dy) + + ( ( (cos(phin) * sin(thetan) * cos(psin) ) + ( sin(phin) * sin(psin) ) ) * dz); + + dy_ENU = (cos(thetan) * sin(psin) * dx) + + ( ( (sin(phin) * sin(thetan) * sin(psin) ) + ( cos(phin) * cos(psin) ) ) * dy) + + ( ( (cos(phin) * sin(thetan) * sin(psin) ) - ( sin(phin) * cos(psin) ) ) * dz); + + dz_ENU = (-sin(thetan) * dx) + ( sin(phin) * cos(thetan) * dy) + + (cos(phin) * cos(thetan) * dz); + + + //Nonlinear state propagation + x = x + TsP * dx_ENU; + y = y + TsP * dy_ENU; + z = z + TsP * dz_ENU; + dx_ENU = dx_ENU + TsP * (u_T_private_ * (1/m_private_) * (cos(psin) * sin(thetan) * cos(phin) + sin(psin) * sin(thetan))); + dy_ENU = dy_ENU + TsP * (u_T_private_ * (1/m_private_) * (sin(psin) * sin(thetan) * cos(phin) - cos(psin) * sin(phin))); + dz_ENU = dz_ENU + TsP * (-g_private_ + u_T_private_ * (1/m_private_) * (cos(thetan) * cos(phin))); + + // Prediction error Matrix + P_ = A_private_*(P_)*A_private_.transpose() + Qp_std_; + + //The predicted state + Xp_ << x, y, z, dx, dy, dz; + +} + +void ExtendedKalmanFilter::AttitudeAddingNoise(double *phin, double *thetan, double* psin, double phi, double theta, double psi){ + assert(phi); + assert(theta); + assert(psi); + + unsigned seed = std::chrono::system_clock::now().time_since_epoch().count(); + std::default_random_engine generatorPhi (seed); + *phin = phi + distribution_(generatorPhi); + + seed = std::chrono::system_clock::now().time_since_epoch().count(); + std::default_random_engine generatorTheta (seed); + *thetan = theta + distribution_(generatorTheta); + + seed = std::chrono::system_clock::now().time_since_epoch().count(); + std::default_random_engine generatorPsi (seed); + *psin = psi + distribution_(generatorPsi); + +} + + +void ExtendedKalmanFilter::Correct(){ + + Eigen::VectorXf Meas(6); + + Meas<< odometry_private_.position[0], + odometry_private_.position[1], + odometry_private_.position[2], + odometry_private_.velocity[0], + odometry_private_.velocity[1], + odometry_private_.velocity[2]; + + + Eigen::MatrixXf K(6,6); + K = P_ * Hp_ * (Hp_.transpose() * P_ * Hp_ + Rp_std_).inverse(); + + Pe_ = P_ - K * Hp_.transpose() * P_; + + Xe_ = Xp_ + K * (Meas - Hp_ * Xp_); + +} + + } diff --git a/src/library/position_controller.cpp b/src/library/position_controller.cpp index da17056..eb46fff 100644 --- a/src/library/position_controller.cpp +++ b/src/library/position_controller.cpp @@ -61,7 +61,22 @@ PositionController::PositionController() e_psi_(0), dot_e_phi_(0), dot_e_theta_(0), - dot_e_psi_(0){ + dot_e_psi_(0), + control_({0,0,0,0}), //pitch, roll, yaw rate, thrust + state_({0, //Position.x + 0, //Position.y + 0, //Position.z + 0, //Linear velocity x + 0, //Linear velocity y + 0, //Linear velocity z + 0, //Quaternion x + 0, //Quaternion y + 0, //Quaternion z + 0, //Quaternion w + 0, //Angular velocity x + 0, //Angular velocity y + 0}) //Angular velocity z) + { timer1_ = n1_.createTimer(ros::Duration(TsA), &PositionController::CallbackAttitude, this, false, true); timer2_ = n2_.createTimer(ros::Duration(TsP), &PositionController::CallbackPosition, this, false, true); @@ -89,8 +104,8 @@ PositionController::PositionController() clientAttitude_ = clientHandleAttitude_.serviceClient("/gazebo/get_world_properties"); clientPosition_ = clientHandlePosition_.serviceClient("/gazebo/get_world_properties"); - ros::WallTime beginWallOffset = ros::WallTime::now(); - wallSecsOffset_ = beginWallOffset.toSec(); + ros::WallTime beginWallOffset = ros::WallTime::now(); + wallSecsOffset_ = beginWallOffset.toSec(); } @@ -249,12 +264,42 @@ void PositionController::SetVehicleParameters(){ Iy_ = vehicle_parameters_.inertia_(1,1); Iz_ = vehicle_parameters_.inertia_(2,2); + extended_kalman_filter_bebop_.SetVehicleParameters(m_, g_); + +} + +void PositionController::SetFilterParameters(){ + + extended_kalman_filter_bebop_.SetFilterParameters(&filter_parameters_); + +} + +void PositionController::Quaternion2Euler(double* roll, double* pitch, double* yaw) const { + assert(roll); + assert(pitch); + assert(yaw); + + double x, y, z, w; + x = odometry_.orientation.x(); + y = odometry_.orientation.y(); + z = odometry_.orientation.z(); + w = odometry_.orientation.w(); + + tf::Quaternion q(x, y, z, w); + tf::Matrix3x3 m(q); + m.getRPY(*roll, *pitch, *yaw); + } void PositionController::SetOdometry(const EigenOdometry& odometry) { odometry_ = odometry; - SetOdometryEstimated(); + + Quaternion2Euler(&state_.attitude.roll, &state_.attitude.pitch, &state_.attitude.yaw); + + state_.angularVelocity.x = odometry_.angular_velocity[0]; + state_.angularVelocity.y = odometry_.angular_velocity[1]; + state_.angularVelocity.z = odometry_.angular_velocity[2]; } @@ -265,9 +310,17 @@ void PositionController::SetTrajectoryPoint(const mav_msgs::EigenTrajectoryPoint } +void PositionController::GetOdometry(nav_msgs::Odometry* odometry_filtered){ + + *odometry_filtered = odometry_filtered_private_; + +} + void PositionController::SetOdometryEstimated() { - extended_kalman_filter_bebop_.Estimator(&state_, &odometry_); + extended_kalman_filter_bebop_.SetThrustCommand(control_.thrust); + extended_kalman_filter_bebop_.Estimator(&state_, &odometry_, &odometry_filtered_private_); + } void PositionController::CalculateRotorVelocities(Eigen::Vector4d* rotor_velocities) { @@ -279,15 +332,15 @@ void PositionController::CalculateRotorVelocities(Eigen::Vector4d* rotor_velocit return; } - double u_T, u_phi, u_theta, u_psi; + double u_phi, u_theta, u_psi; double u_x, u_y, u_Terr; AttitudeController(&u_phi, &u_theta, &u_psi); - PosController(&u_x, &u_y, &u_T, &u_Terr); + PosController(&u_x, &u_y, &control_.thrust, &u_Terr); if(dataStoring_active_){ //Saving control signals in a file std::stringstream tempControlSignals; - tempControlSignals << u_T << "," << u_phi << "," << u_theta << "," << u_psi << "," << u_x << "," << u_y << "," << u_Terr << "," << odometry_.timeStampSec << "," << odometry_.timeStampNsec << "\n"; + tempControlSignals << control_.thrust << "," << u_phi << "," << u_theta << "," << u_psi << "," << u_x << "," << u_y << "," << u_Terr << "," << odometry_.timeStampSec << "," << odometry_.timeStampNsec << "\n"; listControlSignals_.push_back(tempControlSignals.str()); @@ -300,7 +353,7 @@ void PositionController::CalculateRotorVelocities(Eigen::Vector4d* rotor_velocit } double first, second, third, fourth; - first = (1/ ( 4 * bf_ )) * u_T; + first = (1/ ( 4 * bf_ )) * control_.thrust; second = (1/ (4 * bf_ * l_ * cos(M_PI/4) ) ) * u_phi; third = (1/ (4 * bf_ * l_ * cos(M_PI/4) ) ) * u_theta; fourth = (1/ ( 4 * bf_ * bm_)) * u_psi; @@ -406,29 +459,10 @@ void PositionController::VelocityErrors(double* dot_e_x, double* dot_e_y, double x_r = command_trajectory_.position_W[0]; y_r = command_trajectory_.position_W[1]; z_r = command_trajectory_.position_W[2]; - - //The linear velocities are expressed in the inertial body frame. - double dot_x, dot_y, dot_z, theta, psi, phi; - theta = state_.attitude.pitch; - psi = state_.attitude.yaw; - phi = state_.attitude.roll; - - dot_x = (cos(theta) * cos(psi) * state_.linearVelocity.x) + - ( ( (sin(phi) * sin(theta) * cos(psi) ) - ( cos(phi) * sin(psi) ) ) * state_.linearVelocity.y) + - ( ( (cos(phi) * sin(theta) * cos(psi) ) + ( sin(phi) * sin(psi) ) ) * state_.linearVelocity.z); - - dot_y = (cos(theta) * sin(psi) * state_.linearVelocity.x) + - ( ( (sin(phi) * sin(theta) * sin(psi) ) + ( cos(phi) * cos(psi) ) ) * state_.linearVelocity.y) + - ( ( (cos(phi) * sin(theta) * sin(psi) ) - ( sin(phi) * cos(psi) ) ) * state_.linearVelocity.z); - - dot_z = (-sin(theta) * state_.linearVelocity.x) + ( sin(phi) * cos(theta) * state_.linearVelocity.y) + - (cos(phi) * cos(theta) * state_.linearVelocity.z); - - - *dot_e_x = - dot_x; - *dot_e_y = - dot_y; - *dot_e_z = - dot_z; + *dot_e_x = - state_.linearVelocity.x; + *dot_e_y = - state_.linearVelocity.y; + *dot_e_z = - state_.linearVelocity.z; } @@ -550,7 +584,8 @@ void PositionController::CallbackAttitude(const ros::TimerEvent& event){ } void PositionController::CallbackPosition(const ros::TimerEvent& event){ - + + SetOdometryEstimated(); PositionErrors(&e_x_, &e_y_, &e_z_); VelocityErrors(&dot_e_x_, &dot_e_y_, &dot_e_z_); diff --git a/src/nodes/position_controller_node.cpp b/src/nodes/position_controller_node.cpp index f83bbff..9d50a18 100644 --- a/src/nodes/position_controller_node.cpp +++ b/src/nodes/position_controller_node.cpp @@ -23,6 +23,7 @@ #include "position_controller_node.h" #include "teamsannio_med_control/parameters_ros.h" +#include "teamsannio_msgs/default_topics.h" namespace teamsannio_med_control { @@ -40,6 +41,12 @@ PositionControllerNode::PositionControllerNode() { motor_velocity_reference_pub_ = nh.advertise(mav_msgs::default_topics::COMMAND_ACTUATORS, 1); + odometry_sub_gt_ = nh.subscribe(teamsannio_msgs::default_topics::ODOMETRY_GT, 1, &PositionControllerNode::OdometryGTCallback, this); + + odometry_filtered_pub_ = nh.advertise(teamsannio_msgs::default_topics::FILTERED_OUTPUT, 1); + + filtered_errors_pub_ = nh.advertise(teamsannio_msgs::default_topics::STATE_ERRORS, 1); + } PositionControllerNode::~PositionControllerNode(){} @@ -117,14 +124,96 @@ void PositionControllerNode::InitializeParams() { GetVehicleParameters(pnh, &position_controller_.vehicle_parameters_); + GetRosParameter(pnh, "dev_x", + position_controller_.filter_parameters_.dev_x_, + &position_controller_.filter_parameters_.dev_x_); + + GetRosParameter(pnh, "dev_y", + position_controller_.filter_parameters_.dev_y_, + &position_controller_.filter_parameters_.dev_y_); + + GetRosParameter(pnh, "dev_z", + position_controller_.filter_parameters_.dev_z_, + &position_controller_.filter_parameters_.dev_z_); + + GetRosParameter(pnh, "dev_vx", + position_controller_.filter_parameters_.dev_vx_, + &position_controller_.filter_parameters_.dev_vx_); + + GetRosParameter(pnh, "dev_vy", + position_controller_.filter_parameters_.dev_vy_, + &position_controller_.filter_parameters_.dev_vy_); + + GetRosParameter(pnh, "dev_vz", + position_controller_.filter_parameters_.dev_vz_, + &position_controller_.filter_parameters_.dev_vz_); + + GetRosParameter(pnh, "Qp/aa", + position_controller_.filter_parameters_.Qp_x_, + &position_controller_.filter_parameters_.Qp_x_); + + GetRosParameter(pnh, "Qp/bb", + position_controller_.filter_parameters_.Qp_y_, + &position_controller_.filter_parameters_.Qp_y_); + + GetRosParameter(pnh, "Qp/cc", + position_controller_.filter_parameters_.Qp_z_, + &position_controller_.filter_parameters_.Qp_z_); + + GetRosParameter(pnh, "Qp/dd", + position_controller_.filter_parameters_.Qp_vx_, + &position_controller_.filter_parameters_.Qp_vx_); + + GetRosParameter(pnh, "Qp/ee", + position_controller_.filter_parameters_.Qp_vy_, + &position_controller_.filter_parameters_.Qp_vy_); + + GetRosParameter(pnh, "Qp/ff", + position_controller_.filter_parameters_.Qp_vz_, + &position_controller_.filter_parameters_.Qp_vz_); + + position_controller_.filter_parameters_.Rp_(0,0) = position_controller_.filter_parameters_.dev_x_; + position_controller_.filter_parameters_.Rp_(1,1) = position_controller_.filter_parameters_.dev_y_; + position_controller_.filter_parameters_.Rp_(2,2) = position_controller_.filter_parameters_.dev_z_; + position_controller_.filter_parameters_.Rp_(3,3) = position_controller_.filter_parameters_.dev_vx_; + position_controller_.filter_parameters_.Rp_(4,4) = position_controller_.filter_parameters_.dev_vy_; + position_controller_.filter_parameters_.Rp_(5,5) = position_controller_.filter_parameters_.dev_vz_; + + position_controller_.filter_parameters_.Qp_(0,0) = position_controller_.filter_parameters_.Qp_x_; + position_controller_.filter_parameters_.Qp_(1,1) = position_controller_.filter_parameters_.Qp_y_; + position_controller_.filter_parameters_.Qp_(2,2) = position_controller_.filter_parameters_.Qp_z_; + position_controller_.filter_parameters_.Qp_(3,3) = position_controller_.filter_parameters_.Qp_vx_; + position_controller_.filter_parameters_.Qp_(4,4) = position_controller_.filter_parameters_.Qp_vy_; + position_controller_.filter_parameters_.Qp_(5,5) = position_controller_.filter_parameters_.Qp_vz_; + position_controller_.SetControllerGains(); position_controller_.SetVehicleParameters(); + position_controller_.SetFilterParameters(); } void PositionControllerNode::Publish(){ } +void PositionControllerNode::OdometryGTCallback(const nav_msgs::OdometryConstPtr& odometry_msg_gt) { + + ROS_INFO_ONCE("PositionController got first odometry ground truth message."); + + if (waypointHasBeenPublished_){ + + EigenOdometry odometry_gt; + eigenOdometryFromMsg(odometry_msg_gt, &odometry_gt); + + odometry_gt_.pose.pose.position.x = odometry_gt.position[0]; + odometry_gt_.pose.pose.position.y = odometry_gt.position[1]; + odometry_gt_.pose.pose.position.z = odometry_gt.position[2]; + odometry_gt_.twist.twist.linear.x = odometry_gt.velocity[0]; + odometry_gt_.twist.twist.linear.y = odometry_gt.velocity[1]; + odometry_gt_.twist.twist.linear.z = odometry_gt.velocity[2]; + + } +} + void PositionControllerNode::OdometryCallback(const nav_msgs::OdometryConstPtr& odometry_msg) { ROS_INFO_ONCE("PositionController got first odometry message."); @@ -151,6 +240,23 @@ void PositionControllerNode::OdometryCallback(const nav_msgs::OdometryConstPtr& actuator_msg->header.stamp = odometry_msg->header.stamp; motor_velocity_reference_pub_.publish(actuator_msg); + + nav_msgs::Odometry odometry_filtered; + position_controller_.GetOdometry(&odometry_filtered); + odometry_filtered.header.stamp = odometry_msg->header.stamp; + odometry_filtered_pub_.publish(odometry_filtered); + + nav_msgs::Odometry filtered_errors; + filtered_errors.pose.pose.position.x = odometry_filtered.pose.pose.position.x - odometry_gt_.pose.pose.position.x; + filtered_errors.pose.pose.position.y = odometry_filtered.pose.pose.position.y - odometry_gt_.pose.pose.position.y; + filtered_errors.pose.pose.position.z = odometry_filtered.pose.pose.position.z - odometry_gt_.pose.pose.position.z; + filtered_errors.twist.twist.linear.x = odometry_filtered.twist.twist.linear.x - odometry_gt_.twist.twist.linear.x; + filtered_errors.twist.twist.linear.y = odometry_filtered.twist.twist.linear.y - odometry_gt_.twist.twist.linear.y; + filtered_errors.twist.twist.linear.z = odometry_filtered.twist.twist.linear.z - odometry_gt_.twist.twist.linear.z; + + filtered_errors.header.stamp = odometry_msg->header.stamp; + filtered_errors_pub_.publish(filtered_errors); + } } diff --git a/src/nodes/position_controller_node.h b/src/nodes/position_controller_node.h index f2c4a82..76abf68 100644 --- a/src/nodes/position_controller_node.h +++ b/src/nodes/position_controller_node.h @@ -34,6 +34,8 @@ #include "teamsannio_med_control/common.h" #include "teamsannio_med_control/position_controller.h" +#include "teamsannio_med_control/parameters_ros.h" +#include "teamsannio_med_control/parameters.h" namespace teamsannio_med_control { @@ -56,16 +58,20 @@ namespace teamsannio_med_control { //subscribers ros::Subscriber cmd_multi_dof_joint_trajectory_sub_; ros::Subscriber odometry_sub_; + ros::Subscriber odometry_sub_gt_; //publisher ros::Publisher motor_velocity_reference_pub_; + ros::Publisher odometry_filtered_pub_; + ros::Publisher filtered_errors_pub_; mav_msgs::EigenTrajectoryPointDeque commands_; + nav_msgs::Odometry odometry_gt_; std::deque command_waiting_times_; ros::Timer command_timer_; void MultiDofJointTrajectoryCallback(const trajectory_msgs::MultiDOFJointTrajectoryConstPtr& trajectory_reference_msg); - + void OdometryGTCallback(const nav_msgs::OdometryConstPtr& odometry_msg_gt); void OdometryCallback(const nav_msgs::OdometryConstPtr& odometry_msg);