Merge pull request #1 from gsilano/feature/kalman_filter

Extended Kalman Filter

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
 

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"
 )

include/teamsannio_med_control/extendedKalmanFilter.h

@@ -19,32 +19,58 @@
 #ifndef _ESTIMATOR_EXTENDED_KALMAN_FILTER_H_
 #define _ESTIMATOR_EXTENDED_KALMAN_FILTER_H_
 
-#include <mav_msgs/eigen_mav_msgs.h>
 #include "teamsannio_med_control/transform_datatypes.h"
 #include "teamsannio_med_control/Matrix3x3.h"
 #include "teamsannio_med_control/Quaternion.h"
+
 #include <nav_msgs/Odometry.h>
 #include <mav_msgs/conversions.h>
+#include <Eigen/Eigen>
+#include <mav_msgs/eigen_mav_msgs.h>
+
+#include <random>
+#include <cmath>
 
 #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<double> 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();
 
 };
 

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 <Eigen/Eigen>
+#include <ros/ros.h>
+
+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_

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_
 

include/teamsannio_med_control/position_controller.h

@@ -28,6 +28,7 @@
 #include <ros/time.h>
 
 #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_);

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;
 

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_ */

launch/task1_world.launch

@@ -36,9 +36,13 @@
     <node name="position_controller_node" pkg="teamsannio_med_control" type="position_controller_node" output="screen"> 
       <rosparam command="load" file="$(find teamsannio_med_control)/resource/controller_bebop.yaml" />
       <rosparam command="load" file="$(find teamsannio_med_control)/resource/bebop.yaml" />
+      <rosparam command="load" file="$(find teamsannio_med_control)/resource/EKF_matrix.yaml" />
       <param name="use_sim_time" value="$(arg use_sim_time)" />
       <remap from="/command/motor_speed" to="/gazebo/command/motor_speed" />
       <remap from="/odometry" to="/bebop/odometry" />
+      <remap from="/odometry_gt" to="/bebop/odometry_gt" />
+      <remap from="/filteredOutput" to="/bebop/filteredOutput" />
+      <remap from="/stateErrors" to="/bebop/stateErrors" />
       <remap from="/command/trajectory" to="/bebop/command/trajectory" />
     </node>
 
@@ -50,4 +54,11 @@
     <!-- Launch the trajectory plot -->
     <node name="position_plot" pkg="rqt_plot" type="rqt_plot" args="--clear-config /$(arg name)/odometry/pose/pose/position" output="screen" />
 
+    <!-- Launch the filtered output -->
+    <node name="position_plot_gt" pkg="rqt_plot" type="rqt_plot" args="--clear-config /$(arg name)/filteredOutput/pose/pose/position" output="screen" />
+
+    <!-- Launch the state errors -->
+    <node name="state_errors_position_plot_gt" pkg="rqt_plot" type="rqt_plot" args="--clear-config /$(arg name)/stateErrors/pose/pose/position" output="screen" />
+    <node name="state_errors_velocity_plot_gt" pkg="rqt_plot" type="rqt_plot" args="--clear-config /$(arg name)/stateErrors/twist/twist/linear" output="screen" />
+
 </launch>

package.xml

@@ -1,7 +1,7 @@
 <package>
 
   <name>teamsannio_med_control</name>
-  <version>1.0.0</version>
+  <version>0.1.4</version>
   <description>This package provides the Bebop control algorithm.</description>
 
   <maintainer email="giuseppe.silano@unisannio.it">Giuseppe Silano</maintainer>
@@ -14,6 +14,9 @@
 
   <license>Apache</license>
 
+  <url type="website">https://github.com/gsilano/teamsannio_med_control</url>
+  <url type="bugtracker">https://github.com/gsilano/teamsannio_med_control/issues</url>
+
   <buildtool_depend>catkin</buildtool_depend>
 
   <!-- Dependencies needed to compile this package. -->

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 
+