Kalman feature ready for the pull request

The kalman filter features is ready for the pull request

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
 

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

@@ -28,6 +28,9 @@
 #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"
@@ -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<double> 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();

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_;

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

@@ -40,6 +40,9 @@
       <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>
 
@@ -51,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

@@ -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 

src/library/extendedKalmanFilter.cpp

@@ -25,11 +25,14 @@
 
 #include <math.h> 
 #include <ros/ros.h>
+#include <chrono>
 
 #include <nav_msgs/Odometry.h>
 #include <mav_msgs/conversions.h>
 #include <ros/console.h>
 
+#include <random>
+
 #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<double>  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);
+
 }	
 
 

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_);
 
 }