From cc2d714963cd2348d57f0c307a5741a82bfbbd59 Mon Sep 17 00:00:00 2001 From: Pasquale Oppido Date: Mon, 4 Jun 2018 10:26:52 +0200 Subject: [PATCH 1/8] EKF Pseudo Code --- src/library/extendedKalmanFilter.cpp | 96 +++++++++++++++++++++++++++- 1 file changed, 93 insertions(+), 3 deletions(-) diff --git a/src/library/extendedKalmanFilter.cpp b/src/library/extendedKalmanFilter.cpp index 9e4aa62..370ff60 100644 --- a/src/library/extendedKalmanFilter.cpp +++ b/src/library/extendedKalmanFilter.cpp @@ -70,20 +70,110 @@ void ExtendedKalmanFilter::Estimator(state_t *state_, EigenOdometry* odometry_){ } void ExtendedKalmanFilter::Quaternion2Euler(double* roll, double* pitch, double* yaw) const { + + +} + + + + void Ekf::Predict(double* u_T, double* hatx, double* Pp) + { assert(roll); assert(pitch); assert(yaw); - double x, y, z, w; + double x, y, z, dx, dy, dz, w; x = odometry_private_.orientation.x(); y = odometry_private_.orientation.y(); z = odometry_private_.orientation.z(); + dx = odometry_private_.velocity[0]; + dy = odometry_private_.velocity[1]; + dz = odometry_private_.velocity[2]; + w = odometry_private_.orientation.w(); tf::Quaternion q(x, y, z, w); tf::Matrix3x3 m(q); m.getRPY(*roll, *pitch, *yaw); -} + // Non linear Position model discretized with forward Euler + x = x + Tsp_ * dx; + y = y + Tsp_ * dy; + z = z + Tsp_ * dz; + dx = dx + Tsp_ * (*u_T/m_ * (cos(state_.attitude.yaw) * sin(state_.attitude.pitch) * cos(state_.attitude.roll) + sin(state_.attitude.yaw) * sin(state_.attitude.pitch))); + dy = dy + Tsp_ * (*u_T/m_ * (sin(state_.attitude.yaw) * sin(state_.attitude.pitch) * cos(state_.attitude.roll) - cos(state_.attitude.yaw) * sin(state_.attitude.roll))); + dx = dx + Tsp_ * (-g_ + *u_T/m_ * (cos(state_.attitude.pitch) * cos(state_.attitude.roll)); + + // Jacobian Matrix + Matrix6f A; + + A << 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; + + Matrix6f Qp; + + Qp << 0.000001, 0, 0, 0, 0, 0, + 0, 0.000001, 0, 0, 0, 0, + 0, 0, 0.000001, 0, 0, 0, + 0, 0, 0, 0.000001, 0, 0, + 0, 0, 0, 0, 0.000001, 0, + 0, 0, 0, 0, 0, 0.000001; + + Qp = pow(Qp,2); + + // Prediction error Matrix + P = A * Pp * A.transpose() + Qp; + + VectorXd xp(6); + + xp(1) = x; + xp(2) = y; + xp(3) = z; + xp(4) = dx; + xp(5) = dy; + xp(6) = dz; + +} -} + +void EKF::Correct(double* xp, double* P){ + + double x, y, z, dx, dy, dz, w; + x = odometry_private_.orientation.x(); + y = odometry_private_.orientation.y(); + z = odometry_private_.orientation.z(); + dx = odometry_private_.velocity[0]; + dy = odometry_private_.velocity[1]; + dz = odometry_private_.velocity[2]; + + Matrix6f Hp; + + Hp << 1, 0, 0, 0, 0, 0, + 0, 1, 0, 0, 0, 0, + 0, 0, 1, 0, 0, 0, + 0, 0, 0, 1, 0, 0, + 0, 0, 0, 0, 1, 0, + 0, 0, 0, 0, 0, 1; + + Matrix6f Rp; + + Rp << 0.01, 0, 0, 0, 0, 0, + 0, 0.01, 0, 0, 0, 0, + 0, 0, 0.01, 0, 0, 0, + 0, 0, 0, 0.01, 0, 0, + 0, 0, 0, 0, 0.01, 0, + 0, 0, 0, 0, 0, 0.01; + + Rp = pow(Rp,2); + + K = P * Hp * (Hp.transpose() * P * Hp + Rp).inverse(); + + pe = P - K * Hp.transpose() * P; + xe = xp + K * (y - Hp * xp); + + +} \ No newline at end of file From 546d012abdc733e17943b6a4afd7176a66881306 Mon Sep 17 00:00:00 2001 From: Giuseppe Silano Date: Mon, 4 Jun 2018 11:00:43 +0200 Subject: [PATCH 2/8] Updates in the TravisCI file The updates allow building the code into the branch "feature/kalman_filter". --- .travis.yml | 2 ++ 1 file changed, 2 insertions(+) diff --git a/.travis.yml b/.travis.yml index 11d056c..e1ab953 100644 --- a/.travis.yml +++ b/.travis.yml @@ -94,6 +94,8 @@ install: - git checkout med18 - cd ~/catkin_ws/src/mav_comm - git checkout med18 + - cd ~/catkin_ws/src/teamsannio_med_control + - git checkout feature/kalman_filter - cd ~/catkin_ws - catkin build - echo "source ~/catkin_ws/devel/setup.bash" >> ~/.bashrc From e3e39b1882f3f9be8bcd07a57409ccbbb6fbf3ed Mon Sep 17 00:00:00 2001 From: Giuseppe Silano Date: Mon, 4 Jun 2018 11:42:53 +0200 Subject: [PATCH 3/8] Updates in the TravisCI file The part related to notification via mail has been added into the file. --- .travis.yml | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/.travis.yml b/.travis.yml index e1ab953..2554641 100644 --- a/.travis.yml +++ b/.travis.yml @@ -100,3 +100,8 @@ install: - catkin build - echo "source ~/catkin_ws/devel/setup.bash" >> ~/.bashrc - source ~/.bashrc + +notifications: + email: + - giuseppe.silano@unisannio.it + - pasquale.oppido@unisannio.it From 7926c124c267786bd0fff087d898e6f7e6184158 Mon Sep 17 00:00:00 2001 From: Unknown Date: Tue, 5 Jun 2018 14:09:21 +0200 Subject: [PATCH 4/8] Extended Kalman Filter Try 1 code development --- .../extendedKalmanFilter.h | 9 ++ src/library/extendedKalmanFilter.cpp | 139 ++++++++++++------ 2 files changed, 99 insertions(+), 49 deletions(-) diff --git a/include/teamsannio_med_control/extendedKalmanFilter.h b/include/teamsannio_med_control/extendedKalmanFilter.h index d651922..feec110 100644 --- a/include/teamsannio_med_control/extendedKalmanFilter.h +++ b/include/teamsannio_med_control/extendedKalmanFilter.h @@ -42,9 +42,18 @@ class ExtendedKalmanFilter { private: EigenOdometry odometry_private_; + + + + ros::NodeHandle n11_; + ros::Timer t11_; + + void ExtendedKalmanFilter::CallbackEstimate(const ros::TimerEvent& event) void Quaternion2Euler(double* roll, double* pitch, double* yaw) const; void SetOdometry(const EigenOdometry& odometry); + void ExtendedKalmanFilter::correct(Eigen::Matrix6f* xe, Eigen::Matrix6f* pe); + void ExtendedKalmanFilter::predict(Eigen::Vector6f* xp, Eigen::Matrix6f* P); }; diff --git a/src/library/extendedKalmanFilter.cpp b/src/library/extendedKalmanFilter.cpp index 370ff60..ebe13b4 100644 --- a/src/library/extendedKalmanFilter.cpp +++ b/src/library/extendedKalmanFilter.cpp @@ -30,10 +30,17 @@ #include #include +#define TsP 10e-3 /* Position control sampling time */ + + namespace teamsannio_med_control { -ExtendedKalmanFilter::ExtendedKalmanFilter() {} +ExtendedKalmanFilter::ExtendedKalmanFilter() { + + timer11_ = n11_.createTimer(ros::Duration(TsP), &ExtendedKalmanFilter::CallbackEstimate, this, false, true); + +} ExtendedKalmanFilter::~ExtendedKalmanFilter() {} @@ -56,9 +63,6 @@ void ExtendedKalmanFilter::Estimator(state_t *state_, EigenOdometry* odometry_){ 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; @@ -69,88 +73,116 @@ void ExtendedKalmanFilter::Estimator(state_t *state_, EigenOdometry* odometry_){ } -void ExtendedKalmanFilter::Quaternion2Euler(double* roll, double* pitch, double* yaw) const { - - -} +void ExtendedKalmanFilter::SetVehicleParameters(){ + m_ = vehicle_parameters_.mass_; + g_ = vehicle_parameters_.gravity_; + Ix_ = vehicle_parameters_.inertia_(0,0); + Iy_ = vehicle_parameters_.inertia_(1,1); + Iz_ = vehicle_parameters_.inertia_(2,2); +} - void Ekf::Predict(double* u_T, double* hatx, double* Pp) - { +void ExtendedKalmanFilter::Quaternion2Euler(double* roll, double* pitch, double* yaw) const { assert(roll); assert(pitch); assert(yaw); - double x, y, z, dx, dy, dz, w; + double x, y, z, w; x = odometry_private_.orientation.x(); y = odometry_private_.orientation.y(); z = odometry_private_.orientation.z(); - dx = odometry_private_.velocity[0]; - dy = odometry_private_.velocity[1]; - dz = odometry_private_.velocity[2]; - w = odometry_private_.orientation.w(); tf::Quaternion q(x, y, z, w); tf::Matrix3x3 m(q); m.getRPY(*roll, *pitch, *yaw); +} + +void ExtendedKalmanFilter::predict(Eigen::Vector6f* xp, Eigen::Matrix6f* P){ + assert(xp); + assert(P); + + double x, y, z, dx, dy, dz; + x = odometry_private_.position[0]; + y = odometry_private_.position[1]; + z = odometry_private_.position[2]; + dx = odometry_private_.velocity[0]; + dy = odometry_private_.velocity[1]; + dz = odometry_private_.velocity[2]; + + phi = state.attitude.roll; + theta = state.attitude.pitch; + psi = state.attitude.yaw; + + + dot_x = (cos(theta) * cos(psi) * dx) + + ( ( (sin(phi) * sin(theta) * cos(psi) ) - ( cos(phi) * sin(psi) ) ) * dy) + + ( ( (cos(phi) * sin(theta) * cos(psi) ) + ( sin(phi) * sin(psi) ) ) * dz); + + dot_y = (cos(theta) * sin(psi) * dx) + + ( ( (sin(phi) * sin(theta) * sin(psi) ) + ( cos(phi) * cos(psi) ) ) * dy) + + ( ( (cos(phi) * sin(theta) * sin(psi) ) - ( sin(phi) * cos(psi) ) ) * dz); + + dot_z = (-sin(theta) * dx) + ( sin(phi) * cos(theta) * dy) + + (cos(phi) * cos(theta) * dz); + + + double u_x, u_y, u_T, u_Terr; + PositionController::PosController(&u_x, &u_y, &u_T, &u_Terr); + // Non linear Position model discretized with forward Euler - x = x + Tsp_ * dx; - y = y + Tsp_ * dy; - z = z + Tsp_ * dz; - dx = dx + Tsp_ * (*u_T/m_ * (cos(state_.attitude.yaw) * sin(state_.attitude.pitch) * cos(state_.attitude.roll) + sin(state_.attitude.yaw) * sin(state_.attitude.pitch))); - dy = dy + Tsp_ * (*u_T/m_ * (sin(state_.attitude.yaw) * sin(state_.attitude.pitch) * cos(state_.attitude.roll) - cos(state_.attitude.yaw) * sin(state_.attitude.roll))); - dx = dx + Tsp_ * (-g_ + *u_T/m_ * (cos(state_.attitude.pitch) * cos(state_.attitude.roll)); + x = x + Tsp_ * dot_x; + y = y + Tsp_ * dot_y; + z = z + Tsp_ * dot_z; + dot_x = dot_x + Tsp_ * (*u_T/m_ * (cos(psi) * sin(theta) * cos(phi) + sin(psi) * sin(theta))); + dot_y = dot_y + Tsp_ * (*u_T/m_ * (sin(psi) * sin(theta) * cos(phi) - cos(psi) * sin(phi))); + dot_z = dot_z + Tsp_ * (-g_ + *u_T/m_ * (cos(theta) * cos(phi)); // Jacobian Matrix - Matrix6f A; + Eigen::Matrix6f A; A << 1, 0, 0, Tsp_, 0, 0, 0, 1, 0, 0, Tsp_, 0, - 0, 0, 1, 0, 0, Tsp_, + 0, 0, 1, 0, 0, Tsp_, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1; - Matrix6f Qp; - - Qp << 0.000001, 0, 0, 0, 0, 0, - 0, 0.000001, 0, 0, 0, 0, - 0, 0, 0.000001, 0, 0, 0, - 0, 0, 0, 0.000001, 0, 0, - 0, 0, 0, 0, 0.000001, 0, - 0, 0, 0, 0, 0, 0.000001; + Eigen::Matrix6f Qp; + + Qp << 1e-6, 0, 0, 0, 0, 0, + 0, 1e-6, 0, 0, 0, 0, + 0, 0, 1e-6, 0, 0, 0, + 0, 0, 0, 1e-6, 0, 0, + 0, 0, 0, 0, 1e-6, 0, + 0, 0, 0, 0, 0, 1e-6; Qp = pow(Qp,2); // Prediction error Matrix - P = A * Pp * A.transpose() + Qp; + Eigen::MatrixXd P = A * P * A.transpose() + Qp; - VectorXd xp(6); + Eigen::VectorXd xp(6); xp(1) = x; xp(2) = y; xp(3) = z; - xp(4) = dx; - xp(5) = dy; - xp(6) = dz; + xp(4) = dot_x; + xp(5) = dot_y; + xp(6) = dot_z; } -void EKF::Correct(double* xp, double* P){ +void ExtendedKalmanFilter::correct(Eigen::Matrix6f* xe, Eigen::Matrix6f* pe){ - double x, y, z, dx, dy, dz, w; - x = odometry_private_.orientation.x(); - y = odometry_private_.orientation.y(); - z = odometry_private_.orientation.z(); - dx = odometry_private_.velocity[0]; - dy = odometry_private_.velocity[1]; - dz = odometry_private_.velocity[2]; + Eigen::Matrix6f P; + Eigen::Vector6f xp; + predict( &xp, &P); - Matrix6f Hp; + Eigen::Matrix6f Hp; Hp << 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, @@ -159,7 +191,7 @@ void EKF::Correct(double* xp, double* P){ 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1; - Matrix6f Rp; + Eigen::Matrix6f Rp; Rp << 0.01, 0, 0, 0, 0, 0, 0, 0.01, 0, 0, 0, 0, @@ -170,10 +202,19 @@ void EKF::Correct(double* xp, double* P){ Rp = pow(Rp,2); - K = P * Hp * (Hp.transpose() * P * Hp + Rp).inverse(); + Eigen::Matrix6f K = P_ * Hp * (Hp.transpose() * P_ * Hp + Rp).inverse(); - pe = P - K * Hp.transpose() * P; - xe = xp + K * (y - Hp * xp); + Eigen::Matrix6f *pe = P - K * Hp.transpose() * P_; + Eigen::Matrix6f *xe = xp_ + K * (y - Hp * xp_); +} + +void ExtendedKalmanFilter::CallbackEstimate(const ros::TimerEvent& event){ + + predict(&xp, &P); + correct(&xp, &pe); +} + + } \ No newline at end of file From 1f22caea1a212e6d2d11480927077b49e748f117 Mon Sep 17 00:00:00 2001 From: Giuseppe Silano Date: Tue, 5 Jun 2018 14:20:05 +0200 Subject: [PATCH 5/8] Updates in the TravisCI file The updates in the TravisCI file try to overcome the issue occurs when the Xenial distribution of Ubuntu is employed to run the Kinetic Kame version of ROS. --- .travis.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.travis.yml b/.travis.yml index 2554641..02d687d 100644 --- a/.travis.yml +++ b/.travis.yml @@ -66,7 +66,7 @@ env: # Install system dependencies, namely a very barebones ROS setup. before_install: - - sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu `lsb_release -sc` main" > /etc/apt/sources.list.d/ros-latest.list' + - sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu $ROS_CI_DESKTOP main" > /etc/apt/sources.list.d/ros-latest.list' - wget http://packages.ros.org/ros.key -O - | sudo apt-key add - - sudo rm /var/lib/dpkg/lock - sudo dpkg --configure -a From c9d4e64a443d81fc5cca73b277e96fb61685e75b Mon Sep 17 00:00:00 2001 From: Unknown Date: Fri, 8 Jun 2018 18:00:11 +0200 Subject: [PATCH 6/8] Succeded Build EKF The build of the code of EKF is succeded --- .travis.yml | 8 +- CHANGELOG.rst | 7 + .../extendedKalmanFilter.h | 49 ++-- .../filter_parameters.h | 75 ++++++ .../position_controller.h | 28 +- .../teamsannio_med_control/stabilizer_types.h | 6 +- launch/task1_world.launch | 6 + resource/EKF_matrix.yaml | 10 + src/library/extendedKalmanFilter.cpp | 231 ++++++++--------- src/library/position_controller.cpp | 239 ++++++++++++------ src/nodes/position_controller_node.cpp | 63 +++++ src/nodes/position_controller_node.h | 4 - 12 files changed, 480 insertions(+), 246 deletions(-) create mode 100644 include/teamsannio_med_control/filter_parameters.h create mode 100644 resource/EKF_matrix.yaml diff --git a/.travis.yml b/.travis.yml index 02d687d..c2f5da0 100644 --- a/.travis.yml +++ b/.travis.yml @@ -19,12 +19,13 @@ # `wstool`, and should be listed in a file named dependencies.rosinstall. # # There are two variables you may want to change: -# - ROS_DISTRO (default is indigo). Note that packages must be available for -# ubuntu 14.04 trusty. +# - ROS_DISTRO (default is kinetic). Note that packages must be available for +# ubuntu 16.04 trusty. # # See the README.md for more information. # # Author: Felix Duvallet +# Author: Giuseppe Silano # NOTE: The build lifecycle on Travis.ci is something like this: # before_install @@ -94,8 +95,6 @@ install: - git checkout med18 - cd ~/catkin_ws/src/mav_comm - git checkout med18 - - cd ~/catkin_ws/src/teamsannio_med_control - - git checkout feature/kalman_filter - cd ~/catkin_ws - catkin build - echo "source ~/catkin_ws/devel/setup.bash" >> ~/.bashrc @@ -104,4 +103,3 @@ install: notifications: email: - giuseppe.silano@unisannio.it - - pasquale.oppido@unisannio.it diff --git a/CHANGELOG.rst b/CHANGELOG.rst index 660ebd0..0d9c100 100644 --- a/CHANGELOG.rst +++ b/CHANGELOG.rst @@ -2,6 +2,13 @@ Changelog for package teamsannio_med_control ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +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. +* bug fixing +* the sim time has been added in the launch file. Now the position controller node uses the simulation and not wall clock time. +* Contributors: Giuseppe Silano, Luigi Iannelli + 0.1.2 (2018-05-15) ----------- * added the basic.world file inside the repository diff --git a/include/teamsannio_med_control/extendedKalmanFilter.h b/include/teamsannio_med_control/extendedKalmanFilter.h index feec110..4501245 100644 --- a/include/teamsannio_med_control/extendedKalmanFilter.h +++ b/include/teamsannio_med_control/extendedKalmanFilter.h @@ -19,41 +19,54 @@ #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 "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_); + void SetThrustCommand(double u_T); + void SetVehicleParameters(double m, double g); + void SetFilterParameters(FilterParameters *filter_parameters_); private: - EigenOdometry odometry_private_; - - - - ros::NodeHandle n11_; - ros::Timer t11_; - - void ExtendedKalmanFilter::CallbackEstimate(const ros::TimerEvent& event) - - void Quaternion2Euler(double* roll, double* pitch, double* yaw) const; - void SetOdometry(const EigenOdometry& odometry); - void ExtendedKalmanFilter::correct(Eigen::Matrix6f* xe, Eigen::Matrix6f* pe); - void ExtendedKalmanFilter::predict(Eigen::Vector6f* xp, Eigen::Matrix6f* P); + EigenOdometry odometry_private_; + VehicleParameters vehic_param_; + + //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_; + + //Vehicle parameters + double m_private_, g_private_; + double phi_, theta_, psi_; + double u_T_private_; + + void Quaternion2Euler(double* roll, double* pitch, double* yaw) const; + 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..1b42ef0 --- /dev/null +++ b/include/teamsannio_med_control/filter_parameters.h @@ -0,0 +1,75 @@ +/* + * 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. + */ + +#include + +#ifndef _FILTER_PARAMTERS_H_ +#define _FILTER_PARAMTERS_H_ + +static const double DefaultDevX = 0.01; +static const double DefaultDevY = 0.01; +static const double DefaultDevZ = 0.01; + +static const double DefaultDevVX = 0.01; +static const double DefaultDevVY = 0.01; +static const double DefaultDevVZ = 0.01; + +static const double DefaultQpX = 0.01; +static const double DefaultQpY = 0.01; +static const double DefaultQpZ = 0.01; + +static const double DefaultQpVX = 0.01; +static const double DefaultQpVY = 0.01; +static const double DefaultQpVZ = 0.01; + +namespace teamsannio_med_control { + + 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/position_controller.h b/include/teamsannio_med_control/position_controller.h index 0f68c9d..cba1f7f 100644 --- a/include/teamsannio_med_control/position_controller.h +++ b/include/teamsannio_med_control/position_controller.h @@ -28,13 +28,14 @@ #include #include "extendedKalmanFilter.h" +#include "filter_parameters.h" #include "stabilizer_types.h" #include "parameters.h" #include "common.h" -using namespace std; - +#include +using namespace std; namespace teamsannio_med_control { @@ -54,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 @@ -97,15 +96,31 @@ class PositionControllerParameters { void SetTrajectoryPoint(const mav_msgs::EigenTrajectoryPoint& command_trajectory); void SetControllerGains(); void SetVehicleParameters(); + void SetFilterParameters(); PositionControllerParameters controller_parameters_; ExtendedKalmanFilter extended_kalman_filter_bebop_; VehicleParameters vehicle_parameters_; + FilterParameters filter_parameters_; EIGEN_MAKE_ALIGNED_OPERATOR_NEW private: + //Boolean variables to active/unactive the controller and the data storage bool controller_active_; - bool dataStoring_active_; + bool dataStoring_active_; + + //Wall clock time offset variable + double wallSecsOffset_; + + //Gazebo Message for attitude and position + gazebo_msgs::GetWorldProperties my_messagePosition_; + ros::NodeHandle clientHandlePosition_; + ros::ServiceClient clientPosition_; + + ros::NodeHandle clientHandleAttitude_; + ros::ServiceClient clientAttitude_; + gazebo_msgs::GetWorldProperties my_messageAttitude_; + //Sting vectors used to stare data std::vector listControlSignals_; @@ -117,6 +132,8 @@ class PositionControllerParameters { std::vector listTrajectoryErrors_; std::vector listAttitudeErrors_; std::vector listDerivativeAttitudeErrors_; + std::vector listTimeAttitudeErrors_; + std::vector listTimePositionErrors_; //Controller gains double beta_x_, beta_y_, beta_z_; @@ -166,6 +183,7 @@ class PositionControllerParameters { 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..04a0838 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 yaw; double thrust; } control_t; diff --git a/launch/task1_world.launch b/launch/task1_world.launch index 79a458e..3ba2011 100644 --- a/launch/task1_world.launch +++ b/launch/task1_world.launch @@ -3,6 +3,7 @@ + @@ -35,6 +36,8 @@ + + @@ -45,4 +48,7 @@ + + + diff --git a/resource/EKF_matrix.yaml b/resource/EKF_matrix.yaml new file mode 100644 index 0000000..3fbcfa6 --- /dev/null +++ b/resource/EKF_matrix.yaml @@ -0,0 +1,10 @@ +# Tuning matrix of the Extended Kalman Filter +Qp: {aa: 1e-6, bb: 1e-6, cc: 1e-6, dd: 1e-6, ee: 1e-6, ff: 1e-6} +# 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 ebe13b4..feb8e0c 100644 --- a/src/library/extendedKalmanFilter.cpp +++ b/src/library/extendedKalmanFilter.cpp @@ -32,13 +32,29 @@ #define TsP 10e-3 /* Position control sampling time */ +namespace teamsannio_med_control { +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), + Hp_(Eigen::MatrixXf::Identity(6,6)){ -namespace teamsannio_med_control { + + 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; -ExtendedKalmanFilter::ExtendedKalmanFilter() { + Qp_std_ = Qp_private_.transpose()*Qp_private_; + + Rp_std_ = Rp_private_.transpose()*Rp_private_; - timer11_ = n11_.createTimer(ros::Duration(TsP), &ExtendedKalmanFilter::CallbackEstimate, this, false, true); } @@ -49,38 +65,29 @@ void ExtendedKalmanFilter::SetOdometry(const EigenOdometry& odometry) { odometry_private_ = odometry; } +void ExtendedKalmanFilter::SetFilterParameters(FilterParameters *filter_parameters_){ + + Rp_private_ = filter_parameters_->Rp_; + Qp_private_ = filter_parameters_->Qp_; +} + void ExtendedKalmanFilter::Estimator(state_t *state_, EigenOdometry* odometry_){ 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]; - - 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]; - -} - -void ExtendedKalmanFilter::SetVehicleParameters(){ + Predict(); + Correct(); - m_ = vehicle_parameters_.mass_; - g_ = vehicle_parameters_.gravity_; - Ix_ = vehicle_parameters_.inertia_(0,0); - Iy_ = vehicle_parameters_.inertia_(1,1); - Iz_ = vehicle_parameters_.inertia_(2,2); + 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); + } void ExtendedKalmanFilter::Quaternion2Euler(double* roll, double* pitch, double* yaw) const { @@ -100,121 +107,87 @@ void ExtendedKalmanFilter::Quaternion2Euler(double* roll, double* pitch, double* } -void ExtendedKalmanFilter::predict(Eigen::Vector6f* xp, Eigen::Matrix6f* P){ - assert(xp); - assert(P); + +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(){ + + Quaternion2Euler(&phi_, &theta_, &psi_); double x, y, z, dx, dy, dz; - x = odometry_private_.position[0]; - y = odometry_private_.position[1]; - z = odometry_private_.position[2]; - dx = odometry_private_.velocity[0]; - dy = odometry_private_.velocity[1]; - dz = odometry_private_.velocity[2]; - - phi = state.attitude.roll; - theta = state.attitude.pitch; - psi = state.attitude.yaw; - - - dot_x = (cos(theta) * cos(psi) * dx) + - ( ( (sin(phi) * sin(theta) * cos(psi) ) - ( cos(phi) * sin(psi) ) ) * dy) + - ( ( (cos(phi) * sin(theta) * cos(psi) ) + ( sin(phi) * sin(psi) ) ) * dz); - - dot_y = (cos(theta) * sin(psi) * dx) + - ( ( (sin(phi) * sin(theta) * sin(psi) ) + ( cos(phi) * cos(psi) ) ) * dy) + - ( ( (cos(phi) * sin(theta) * sin(psi) ) - ( sin(phi) * cos(psi) ) ) * dz); - - dot_z = (-sin(theta) * dx) + ( sin(phi) * cos(theta) * dy) + - (cos(phi) * cos(theta) * dz); - - - double u_x, u_y, u_T, u_Terr; - PositionController::PosController(&u_x, &u_y, &u_T, &u_Terr); - - // Non linear Position model discretized with forward Euler - x = x + Tsp_ * dot_x; - y = y + Tsp_ * dot_y; - z = z + Tsp_ * dot_z; - dot_x = dot_x + Tsp_ * (*u_T/m_ * (cos(psi) * sin(theta) * cos(phi) + sin(psi) * sin(theta))); - dot_y = dot_y + Tsp_ * (*u_T/m_ * (sin(psi) * sin(theta) * cos(phi) - cos(psi) * sin(phi))); - dot_z = dot_z + Tsp_ * (-g_ + *u_T/m_ * (cos(theta) * cos(phi)); - - // Jacobian Matrix - Eigen::Matrix6f A; - - A << 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; - - Eigen::Matrix6f Qp; - - Qp << 1e-6, 0, 0, 0, 0, 0, - 0, 1e-6, 0, 0, 0, 0, - 0, 0, 1e-6, 0, 0, 0, - 0, 0, 0, 1e-6, 0, 0, - 0, 0, 0, 0, 1e-6, 0, - 0, 0, 0, 0, 0, 1e-6; - - Qp = pow(Qp,2); - - // Prediction error Matrix - Eigen::MatrixXd P = A * P * A.transpose() + Qp; + 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; - Eigen::VectorXd xp(6); + dx_ENU = (cos(theta_) * cos(psi_) * dx) + + ( ( (sin(phi_) * sin(theta_) * cos(psi_) ) - ( cos(phi_) * sin(psi_) ) ) * dy) + + ( ( (cos(phi_) * sin(theta_) * cos(psi_) ) + ( sin(phi_) * sin(psi_) ) ) * dz); + + dy_ENU = (cos(theta_) * sin(psi_) * dx) + + ( ( (sin(phi_) * sin(theta_) * sin(psi_) ) + ( cos(phi_) * cos(psi_) ) ) * dy) + + ( ( (cos(phi_) * sin(theta_) * sin(psi_) ) - ( sin(phi_) * cos(psi_) ) ) * dz); + + dz_ENU = (-sin(theta_) * dx) + ( sin(phi_) * cos(theta_) * dy) + + (cos(phi_) * cos(theta_) * 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(psi_) * sin(theta_) * cos(phi_) + sin(psi_) * sin(theta_))); + dy_ENU = dy_ENU + TsP * (u_T_private_ * (1/m_private_) * (sin(psi_) * sin(theta_) * cos(phi_) - cos(psi_) * sin(phi_))); + dz_ENU = dz_ENU + TsP * (-g_private_ + u_T_private_ * (1/m_private_) * (cos(theta_) * cos(phi_))); - xp(1) = x; - xp(2) = y; - xp(3) = z; - xp(4) = dot_x; - xp(5) = dot_y; - xp(6) = dot_z; - + + // Prediction error Matrix + P_ = A_private_*(P_)*A_private_.transpose() + Qp_std_; + + //The predicted state + Xp_ << x, y, z, dx, dy, dz; + } -void ExtendedKalmanFilter::correct(Eigen::Matrix6f* xe, Eigen::Matrix6f* pe){ - - Eigen::Matrix6f P; - Eigen::Vector6f xp; - predict( &xp, &P); - - Eigen::Matrix6f Hp; - - Hp << 1, 0, 0, 0, 0, 0, - 0, 1, 0, 0, 0, 0, - 0, 0, 1, 0, 0, 0, - 0, 0, 0, 1, 0, 0, - 0, 0, 0, 0, 1, 0, - 0, 0, 0, 0, 0, 1; - - Eigen::Matrix6f Rp; +void ExtendedKalmanFilter::Correct(){ - Rp << 0.01, 0, 0, 0, 0, 0, - 0, 0.01, 0, 0, 0, 0, - 0, 0, 0.01, 0, 0, 0, - 0, 0, 0, 0.01, 0, 0, - 0, 0, 0, 0, 0.01, 0, - 0, 0, 0, 0, 0, 0.01; - - Rp = pow(Rp,2); + 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::Matrix6f K = P_ * Hp * (Hp.transpose() * P_ * Hp + Rp).inverse(); + Eigen::MatrixXf K(6,6); + K = P_ * Hp_ * (Hp_.transpose() * P_ * Hp_ + Rp_std_).inverse(); - Eigen::Matrix6f *pe = P - K * Hp.transpose() * P_; - Eigen::Matrix6f *xe = xp_ + K * (y - Hp * xp_); + Pe_ = P_ - K * Hp_.transpose() * P_; + + Xe_ = Xp_ + K * (Meas - Hp_ * Xp_); } -void ExtendedKalmanFilter::CallbackEstimate(const ros::TimerEvent& event){ - - predict(&xp, &P); - correct(&xp, &pe); -} -} \ No newline at end of file +} diff --git a/src/library/position_controller.cpp b/src/library/position_controller.cpp index 361d1d3..2c656dc 100644 --- a/src/library/position_controller.cpp +++ b/src/library/position_controller.cpp @@ -23,6 +23,7 @@ #include "teamsannio_med_control/stabilizer_types.h" #include +#include #include #include #include @@ -40,7 +41,7 @@ #define M_PI 3.14159265358979323846 /* pi */ #define TsP 10e-3 /* Position control sampling time */ #define TsA 5e-3 /* Attitude control sampling time */ -#define storeTime 3 /* Store time*/ +#define storeTime 15 /* Store time*/ using namespace std; @@ -48,7 +49,7 @@ namespace teamsannio_med_control { PositionController::PositionController() : controller_active_(false), - dataStoring_active_(false), + dataStoring_active_(false), e_x_(0), e_y_(0), e_z_(0), @@ -64,14 +65,15 @@ PositionController::PositionController() timer1_ = n1_.createTimer(ros::Duration(TsA), &PositionController::CallbackAttitude, this, false, true); timer2_ = n2_.createTimer(ros::Duration(TsP), &PositionController::CallbackPosition, this, false, true); - //this serves to inactivate the controller if we don't recieve a trajectory - - if(dataStoring_active_){ - timer3_ = n3_.createTimer(ros::Duration(storeTime), &PositionController::CallbackSaveData, this, false, true); + + + //this serves to inactivate the logging if we wan't to save the simulated data + if(dataStoring_active_){ + timer3_ = n3_.createTimer(ros::Duration(storeTime), &PositionController::CallbackSaveData, this, false, true); //Cleaning the string vector contents listControlSignals_.clear(); - listControlSignals_.clear(); + listControlSignals_.clear(); listControlMixerTerms_.clear(); listPropellersAngularVelocities_.clear(); listReferenceAngles_.clear(); @@ -79,7 +81,17 @@ PositionController::PositionController() listDroneAttitude_.clear(); listTrajectoryErrors_.clear(); listAttitudeErrors_.clear(); - listDerivativeAttitudeErrors_.clear(); + listDerivativeAttitudeErrors_.clear(); + listTimeAttitudeErrors_.clear(); + listTimePositionErrors_.clear(); + + //the client needed to get information about the Gazebo simulation environment both the attitude and position errors + clientAttitude_ = clientHandleAttitude_.serviceClient("/gazebo/get_world_properties"); + clientPosition_ = clientHandlePosition_.serviceClient("/gazebo/get_world_properties"); + + ros::WallTime beginWallOffset = ros::WallTime::now(); + wallSecsOffset_ = beginWallOffset.toSec(); + } @@ -101,21 +113,25 @@ void PositionController::CallbackSaveData(const ros::TimerEvent& event){ ofstream fileTrajectoryErrors; ofstream fileAttitudeErrors; ofstream fileDerivativeAttitudeErrors; + ofstream fileTimeAttitudeErrors; + ofstream fileTimePositionErrors; ROS_INFO("CallbackSavaData function is working. Time: %f seconds, %f nanoseconds", odometry_.timeStampSec, odometry_.timeStampNsec); - fileControllerGains.open ("/home/giuseppe/Scrivania/controllerGains.csv", std::ios_base::app); - fileVehicleParameters.open ("/home/giuseppe/Scrivania/vehicleParamters.csv", std::ios_base::app); - fileControlSignals.open ("/home/giuseppe/Scrivania/controlSignals.csv", std::ios_base::app); - fileControlMixerTerms.open ("/home/giuseppe/Scrivania/controlMixer.csv", std::ios_base::app); - filePropellersAngularVelocities.open ("/home/giuseppe/Scrivania/propellersAngularVelocities.csv", std::ios_base::app); - fileReferenceAngles.open ("/home/giuseppe/Scrivania/referenceAngles.csv", std::ios_base::app); - fileVelocityErrors.open ("/home/giuseppe/Scrivania/velocityErrors.csv", std::ios_base::app); - fileDroneAttiude.open ("/home/giuseppe/Scrivania/droneAttitude.csv", std::ios_base::app); - fileTrajectoryErrors.open ("/home/giuseppe/Scrivania/trajectoryErrors.csv", std::ios_base::app); - fileAttitudeErrors.open ("/home/giuseppe/Scrivania/attitudeErrors.csv", std::ios_base::app); - fileDerivativeAttitudeErrors.open ("/home/giuseppe/Scrivania/derivativeAttitudeErrors.csv", std::ios_base::app); + fileControllerGains.open ("/home/giuseppe/controllerGains.csv", std::ios_base::app); + fileVehicleParameters.open ("/home/giuseppe/vehicleParamters.csv", std::ios_base::app); + fileControlSignals.open ("/home/giuseppe/controlSignals.csv", std::ios_base::app); + fileControlMixerTerms.open ("/home/giuseppe/controlMixer.csv", std::ios_base::app); + filePropellersAngularVelocities.open ("/home/giuseppe/propellersAngularVelocities.csv", std::ios_base::app); + fileReferenceAngles.open ("/home/giuseppe/referenceAngles.csv", std::ios_base::app); + fileVelocityErrors.open ("/home/giuseppe/velocityErrors.csv", std::ios_base::app); + fileDroneAttiude.open ("/home/giuseppe/droneAttitude.csv", std::ios_base::app); + fileTrajectoryErrors.open ("/home/giuseppe/trajectoryErrors.csv", std::ios_base::app); + fileAttitudeErrors.open ("/home/giuseppe/attitudeErrors.csv", std::ios_base::app); + fileDerivativeAttitudeErrors.open ("/home/giuseppe/derivativeAttitudeErrors.csv", std::ios_base::app); + fileTimeAttitudeErrors.open ("/home/giuseppe/timeAttitudeErrors.csv", std::ios_base::app); + fileTimePositionErrors.open ("/home/giuseppe/timePositionErrors.csv", std::ios_base::app); //Saving vehicle parameters in a file fileControllerGains << beta_x_ << "," << beta_y_ << "," << beta_z_ << "," << alpha_x_ << "," << alpha_y_ << "," << alpha_z_ << "," << beta_phi_ << "," << beta_theta_ << "," << beta_psi_ << "," << alpha_phi_ << "," << alpha_theta_ << "," << alpha_psi_ << "," << mu_x_ << "," << mu_y_ << "," << mu_z_ << "," << mu_phi_ << "," << mu_theta_ << "," << mu_psi_ << "," << odometry_.timeStampSec << "," << odometry_.timeStampNsec << "\n"; @@ -143,7 +159,7 @@ void PositionController::CallbackSaveData(const ros::TimerEvent& event){ fileReferenceAngles << listReferenceAngles_.at( n ); } - //Saving the velcoty errors in a file + //Saving the velocity errors in a file for (unsigned n=0; n < listVelocityErrors_.size(); ++n) { fileVelocityErrors << listVelocityErrors_.at( n ); } @@ -168,6 +184,14 @@ void PositionController::CallbackSaveData(const ros::TimerEvent& event){ fileDerivativeAttitudeErrors << listDerivativeAttitudeErrors_.at( n ); } + //Saving the position and attitude errors along the time + for (unsigned n=0; n < listTimeAttitudeErrors_.size(); ++n) { + fileTimeAttitudeErrors << listTimeAttitudeErrors_.at( n ); + } + + for (unsigned n=0; n < listTimePositionErrors_.size(); ++n) { + fileTimePositionErrors << listTimePositionErrors_.at( n ); + } //Closing all opened files fileControllerGains.close (); @@ -181,6 +205,8 @@ void PositionController::CallbackSaveData(const ros::TimerEvent& event){ fileTrajectoryErrors.close(); fileAttitudeErrors.close(); fileDerivativeAttitudeErrors.close(); + fileTimeAttitudeErrors.close(); + fileTimePositionErrors.close(); } @@ -223,12 +249,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]; } @@ -242,6 +298,7 @@ void PositionController::SetTrajectoryPoint(const mav_msgs::EigenTrajectoryPoint void PositionController::SetOdometryEstimated() { extended_kalman_filter_bebop_.Estimator(&state_, &odometry_); + } void PositionController::CalculateRotorVelocities(Eigen::Vector4d* rotor_velocities) { @@ -257,43 +314,22 @@ void PositionController::CalculateRotorVelocities(Eigen::Vector4d* rotor_velocit double u_x, u_y, u_Terr; AttitudeController(&u_phi, &u_theta, &u_psi); PosController(&u_x, &u_y, &u_T, &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"; - - listControlSignals_.push_back(tempControlSignals.str()); - - //Saving drone attitude in a file - std::stringstream tempDroneAttitude; - tempDroneAttitude << state_.attitude.roll << "," << state_.attitude.pitch << "," << state_.attitude.yaw << "," < command_waiting_times_; ros::Timer command_timer_; - void TimedCommandCallback(const ros::TimerEvent& e); - void MultiDofJointTrajectoryCallback(const trajectory_msgs::MultiDOFJointTrajectoryConstPtr& trajectory_reference_msg); - void CommandPoseCallback(const geometry_msgs::PoseStampedConstPtr& pose_msg); - void OdometryCallback(const nav_msgs::OdometryConstPtr& odometry_msg); From 02220429df6b779451cdaa43d62e7a6f0cd0bf84 Mon Sep 17 00:00:00 2001 From: Giuseppe Silano Date: Sat, 9 Jun 2018 18:09:28 +0200 Subject: [PATCH 7/8] Bug fixed The bug appears when the roslaunch command is executed has been fixed --- .../extendedKalmanFilter.h | 2 - .../filter_parameters.h | 85 ++++++++++--------- include/teamsannio_med_control/parameters.h | 18 ++++ .../position_controller.h | 1 + .../teamsannio_med_control/stabilizer_types.h | 2 +- resource/EKF_matrix.yaml | 2 +- src/library/extendedKalmanFilter.cpp | 45 +++++----- src/library/position_controller.cpp | 32 +++++-- src/nodes/position_controller_node.h | 2 + 9 files changed, 115 insertions(+), 74 deletions(-) diff --git a/include/teamsannio_med_control/extendedKalmanFilter.h b/include/teamsannio_med_control/extendedKalmanFilter.h index 4501245..c2dd13a 100644 --- a/include/teamsannio_med_control/extendedKalmanFilter.h +++ b/include/teamsannio_med_control/extendedKalmanFilter.h @@ -49,7 +49,6 @@ class ExtendedKalmanFilter { private: EigenOdometry odometry_private_; - VehicleParameters vehic_param_; //Filter Vectors Eigen::VectorXf Xp_, Xe_, Hatx_; @@ -60,7 +59,6 @@ class ExtendedKalmanFilter { //Vehicle parameters double m_private_, g_private_; - double phi_, theta_, psi_; double u_T_private_; void Quaternion2Euler(double* roll, double* pitch, double* yaw) const; diff --git a/include/teamsannio_med_control/filter_parameters.h b/include/teamsannio_med_control/filter_parameters.h index 1b42ef0..008d494 100644 --- a/include/teamsannio_med_control/filter_parameters.h +++ b/include/teamsannio_med_control/filter_parameters.h @@ -16,59 +16,60 @@ * limitations under the License. */ -#include - #ifndef _FILTER_PARAMTERS_H_ #define _FILTER_PARAMTERS_H_ -static const double DefaultDevX = 0.01; -static const double DefaultDevY = 0.01; -static const double DefaultDevZ = 0.01; +#include +#include + +namespace teamsannio_med_control { -static const double DefaultDevVX = 0.01; -static const double DefaultDevVY = 0.01; -static const double DefaultDevVZ = 0.01; +static constexpr double DefaultDevX = 0.01; +static constexpr double DefaultDevY = 0.01; +static constexpr double DefaultDevZ = 0.01; -static const double DefaultQpX = 0.01; -static const double DefaultQpY = 0.01; -static const double DefaultQpZ = 0.01; +static constexpr double DefaultDevVX = 0.01; +static constexpr double DefaultDevVY = 0.01; +static constexpr double DefaultDevVZ = 0.01; -static const double DefaultQpVX = 0.01; -static const double DefaultQpVY = 0.01; -static const double DefaultQpVZ = 0.01; +static constexpr double DefaultQpX = 1e-6; +static constexpr double DefaultQpY = 1e-6; +static constexpr double DefaultQpZ = 1e-6; -namespace teamsannio_med_control { +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)){ - } +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_; + Eigen::MatrixXf Rp_, Qp_; - double dev_x_, Qp_x_; - double dev_y_, Qp_y_; - double dev_z_, Qp_z_; + 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_; - }; + double dev_vx_, Qp_vx_; + double dev_vy_, Qp_vy_; + double dev_vz_, Qp_vz_; +}; } 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 cba1f7f..8cc5f4a 100644 --- a/include/teamsannio_med_control/position_controller.h +++ b/include/teamsannio_med_control/position_controller.h @@ -179,6 +179,7 @@ class PositionControllerParameters { void CallbackSaveData(const ros::TimerEvent& event); state_t state_; + control_t control_; mav_msgs::EigenTrajectoryPoint command_trajectory_; EigenOdometry odometry_; diff --git a/include/teamsannio_med_control/stabilizer_types.h b/include/teamsannio_med_control/stabilizer_types.h index 04a0838..c2ece46 100644 --- a/include/teamsannio_med_control/stabilizer_types.h +++ b/include/teamsannio_med_control/stabilizer_types.h @@ -127,7 +127,7 @@ typedef struct state_s { typedef struct control_s { double roll; double pitch; - double yaw; + double yawRate; double thrust; } control_t; diff --git a/resource/EKF_matrix.yaml b/resource/EKF_matrix.yaml index 3fbcfa6..b041d49 100644 --- a/resource/EKF_matrix.yaml +++ b/resource/EKF_matrix.yaml @@ -1,5 +1,5 @@ # Tuning matrix of the Extended Kalman Filter -Qp: {aa: 1e-6, bb: 1e-6, cc: 1e-6, dd: 1e-6, ee: 1e-6, ff: 1e-6} +Qp: {aa: 0.0000001, bb: 0.0000001, cc: 0.0000001, dd: 0.0000001, ee: 0.0000001, ff: 0.0000001} # Standard deviation of noise on positions and linear velocities dev_x: 0.01 dev_y: 0.01 diff --git a/src/library/extendedKalmanFilter.cpp b/src/library/extendedKalmanFilter.cpp index feb8e0c..d31371b 100644 --- a/src/library/extendedKalmanFilter.cpp +++ b/src/library/extendedKalmanFilter.cpp @@ -41,7 +41,14 @@ ExtendedKalmanFilter::ExtendedKalmanFilter() Pe_(Eigen::MatrixXf::Zero(6,6)), Hatx_(Eigen::VectorXf::Zero(6)), u_T_private_(0), - Hp_(Eigen::MatrixXf::Identity(6,6)){ + 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, @@ -51,11 +58,6 @@ ExtendedKalmanFilter::ExtendedKalmanFilter() 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1; - Qp_std_ = Qp_private_.transpose()*Qp_private_; - - Rp_std_ = Rp_private_.transpose()*Rp_private_; - - } ExtendedKalmanFilter::~ExtendedKalmanFilter() {} @@ -69,6 +71,10 @@ void ExtendedKalmanFilter::SetFilterParameters(FilterParameters *filter_paramete 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_){ @@ -123,7 +129,8 @@ void ExtendedKalmanFilter::SetVehicleParameters(double m, double g){ void ExtendedKalmanFilter::Predict(){ - Quaternion2Euler(&phi_, &theta_, &psi_); + double phi, theta, psi; + Quaternion2Euler(&phi, &theta, &psi); double x, y, z, dx, dy, dz; x = Hatx_(0); @@ -136,25 +143,25 @@ void ExtendedKalmanFilter::Predict(){ double dx_ENU, dy_ENU, dz_ENU; - dx_ENU = (cos(theta_) * cos(psi_) * dx) + - ( ( (sin(phi_) * sin(theta_) * cos(psi_) ) - ( cos(phi_) * sin(psi_) ) ) * dy) + - ( ( (cos(phi_) * sin(theta_) * cos(psi_) ) + ( sin(phi_) * sin(psi_) ) ) * dz); + dx_ENU = (cos(theta) * cos(psi) * dx) + + ( ( (sin(phi) * sin(theta) * cos(psi) ) - ( cos(phi) * sin(psi) ) ) * dy) + + ( ( (cos(phi) * sin(theta) * cos(psi) ) + ( sin(phi) * sin(psi) ) ) * dz); - dy_ENU = (cos(theta_) * sin(psi_) * dx) + - ( ( (sin(phi_) * sin(theta_) * sin(psi_) ) + ( cos(phi_) * cos(psi_) ) ) * dy) + - ( ( (cos(phi_) * sin(theta_) * sin(psi_) ) - ( sin(phi_) * cos(psi_) ) ) * dz); + dy_ENU = (cos(theta) * sin(psi) * dx) + + ( ( (sin(phi) * sin(theta) * sin(psi) ) + ( cos(phi) * cos(psi) ) ) * dy) + + ( ( (cos(phi) * sin(theta) * sin(psi) ) - ( sin(phi) * cos(psi) ) ) * dz); - dz_ENU = (-sin(theta_) * dx) + ( sin(phi_) * cos(theta_) * dy) + - (cos(phi_) * cos(theta_) * dz); + dz_ENU = (-sin(theta) * dx) + ( sin(phi) * cos(theta) * dy) + + (cos(phi) * cos(theta) * 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(psi_) * sin(theta_) * cos(phi_) + sin(psi_) * sin(theta_))); - dy_ENU = dy_ENU + TsP * (u_T_private_ * (1/m_private_) * (sin(psi_) * sin(theta_) * cos(phi_) - cos(psi_) * sin(phi_))); - dz_ENU = dz_ENU + TsP * (-g_private_ + u_T_private_ * (1/m_private_) * (cos(theta_) * cos(phi_))); + dx_ENU = dx_ENU + TsP * (u_T_private_ * (1/m_private_) * (cos(psi) * sin(theta) * cos(phi) + sin(psi) * sin(theta))); + dy_ENU = dy_ENU + TsP * (u_T_private_ * (1/m_private_) * (sin(psi) * sin(theta) * cos(phi) - cos(psi) * sin(phi))); + dz_ENU = dz_ENU + TsP * (-g_private_ + u_T_private_ * (1/m_private_) * (cos(theta) * cos(phi))); // Prediction error Matrix @@ -185,9 +192,7 @@ void ExtendedKalmanFilter::Correct(){ Xe_ = Xp_ + K * (Meas - Hp_ * Xp_); - } - } diff --git a/src/library/position_controller.cpp b/src/library/position_controller.cpp index 2c656dc..6a65cfe 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(); } @@ -297,6 +312,7 @@ void PositionController::SetTrajectoryPoint(const mav_msgs::EigenTrajectoryPoint void PositionController::SetOdometryEstimated() { + extended_kalman_filter_bebop_.SetThrustCommand(control_.thrust); extended_kalman_filter_bebop_.Estimator(&state_, &odometry_); } @@ -310,17 +326,17 @@ 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); + - extended_kalman_filter_bebop_.SetThrustCommand(u_T); 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()); @@ -333,7 +349,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; diff --git a/src/nodes/position_controller_node.h b/src/nodes/position_controller_node.h index f2c4a82..94d1c94 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 { From 4c9315d7318b151b70eb970d2a47deefff9f968b Mon Sep 17 00:00:00 2001 From: Giuseppe Silano Date: Mon, 11 Jun 2018 15:34:36 +0200 Subject: [PATCH 8/8] Kalman feature ready for the pull request The kalman filter features is ready for the pull request --- CHANGELOG.rst | 17 +++-- CMakeLists.txt | 1 + .../extendedKalmanFilter.h | 8 ++- .../position_controller.h | 3 + include/teamsannio_msgs/default_topics.h | 33 +++++++++ launch/task1_world.launch | 10 +++ package.xml | 5 +- resource/EKF_matrix.yaml | 4 +- src/library/extendedKalmanFilter.cpp | 72 ++++++++++++++----- src/library/position_controller.cpp | 8 ++- src/nodes/position_controller_node.cpp | 43 +++++++++++ src/nodes/position_controller_node.h | 6 +- 12 files changed, 183 insertions(+), 27 deletions(-) create mode 100644 include/teamsannio_msgs/default_topics.h diff --git a/CHANGELOG.rst b/CHANGELOG.rst index 0d9c100..18313fb 100644 --- a/CHANGELOG.rst +++ b/CHANGELOG.rst @@ -2,27 +2,36 @@ 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. * bug fixing * the sim time has been added in the launch file. Now the position controller node uses the simulation and not wall clock time. * 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 c2dd13a..7caaf08 100644 --- a/include/teamsannio_med_control/extendedKalmanFilter.h +++ b/include/teamsannio_med_control/extendedKalmanFilter.h @@ -28,6 +28,9 @@ #include #include +#include +#include + #include "stabilizer_types.h" #include "filter_parameters.h" #include "common.h" @@ -41,7 +44,7 @@ class 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_); @@ -56,12 +59,15 @@ class ExtendedKalmanFilter { 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 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/position_controller.h b/include/teamsannio_med_control/position_controller.h index 8cc5f4a..11756db 100644 --- a/include/teamsannio_med_control/position_controller.h +++ b/include/teamsannio_med_control/position_controller.h @@ -97,6 +97,7 @@ class PositionControllerParameters { void SetControllerGains(); void SetVehicleParameters(); void SetFilterParameters(); + void GetOdometry(nav_msgs::Odometry* odometry_filtered); PositionControllerParameters controller_parameters_; ExtendedKalmanFilter extended_kalman_filter_bebop_; @@ -178,6 +179,8 @@ 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_; 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 3ba2011..43a12fa 100644 --- a/launch/task1_world.launch +++ b/launch/task1_world.launch @@ -40,6 +40,9 @@ + + + @@ -51,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 index b041d49..ca62474 100644 --- a/resource/EKF_matrix.yaml +++ b/resource/EKF_matrix.yaml @@ -1,5 +1,5 @@ -# Tuning matrix of the Extended Kalman Filter -Qp: {aa: 0.0000001, bb: 0.0000001, cc: 0.0000001, dd: 0.0000001, ee: 0.0000001, ff: 0.0000001} +# 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 diff --git a/src/library/extendedKalmanFilter.cpp b/src/library/extendedKalmanFilter.cpp index d31371b..4856731 100644 --- a/src/library/extendedKalmanFilter.cpp +++ b/src/library/extendedKalmanFilter.cpp @@ -25,11 +25,14 @@ #include #include +#include #include #include #include +#include + #define TsP 10e-3 /* Position control sampling time */ namespace teamsannio_med_control { @@ -58,6 +61,9 @@ ExtendedKalmanFilter::ExtendedKalmanFilter() 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() {} @@ -75,9 +81,10 @@ void ExtendedKalmanFilter::SetFilterParameters(FilterParameters *filter_paramete Qp_std_ = Qp_private_.transpose()*Qp_private_; Rp_std_ = Rp_private_.transpose()*Rp_private_; + } -void ExtendedKalmanFilter::Estimator(state_t *state_, EigenOdometry* odometry_){ +void ExtendedKalmanFilter::Estimator(state_t *state_, EigenOdometry* odometry_, nav_msgs::Odometry* odometry_filtered){ assert(state_); assert(odometry_); @@ -92,8 +99,18 @@ void ExtendedKalmanFilter::Estimator(state_t *state_, EigenOdometry* odometry_){ state_->linearVelocity.x = Xe_(3); state_->linearVelocity.x = Xe_(4); - state_->linearVelocity.x = Xe_(5); - + 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 { @@ -132,6 +149,9 @@ 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); @@ -143,33 +163,51 @@ void ExtendedKalmanFilter::Predict(){ double dx_ENU, dy_ENU, dz_ENU; - dx_ENU = (cos(theta) * cos(psi) * dx) + - ( ( (sin(phi) * sin(theta) * cos(psi) ) - ( cos(phi) * sin(psi) ) ) * dy) + - ( ( (cos(phi) * sin(theta) * cos(psi) ) + ( sin(phi) * sin(psi) ) ) * dz); + 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(theta) * sin(psi) * dx) + - ( ( (sin(phi) * sin(theta) * sin(psi) ) + ( cos(phi) * cos(psi) ) ) * dy) + - ( ( (cos(phi) * sin(theta) * sin(psi) ) - ( sin(phi) * cos(psi) ) ) * 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(theta) * dx) + ( sin(phi) * cos(theta) * dy) + - (cos(phi) * cos(theta) * 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(psi) * sin(theta) * cos(phi) + sin(psi) * sin(theta))); - dy_ENU = dy_ENU + TsP * (u_T_private_ * (1/m_private_) * (sin(psi) * sin(theta) * cos(phi) - cos(psi) * sin(phi))); - dz_ENU = dz_ENU + TsP * (-g_private_ + u_T_private_ * (1/m_private_) * (cos(theta) * cos(phi))); - - + 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); + } diff --git a/src/library/position_controller.cpp b/src/library/position_controller.cpp index 6a65cfe..ba9c46f 100644 --- a/src/library/position_controller.cpp +++ b/src/library/position_controller.cpp @@ -310,10 +310,16 @@ 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_.SetThrustCommand(control_.thrust); - extended_kalman_filter_bebop_.Estimator(&state_, &odometry_); + extended_kalman_filter_bebop_.Estimator(&state_, &odometry_, &odometry_filtered_private_); } diff --git a/src/nodes/position_controller_node.cpp b/src/nodes/position_controller_node.cpp index b87d644..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(){} @@ -188,6 +195,25 @@ void PositionControllerNode::InitializeParams() { 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."); @@ -214,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 94d1c94..76abf68 100644 --- a/src/nodes/position_controller_node.h +++ b/src/nodes/position_controller_node.h @@ -58,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);