From d9e88436a83bbe58fdf971cd0ed5edec86c7f7e0 Mon Sep 17 00:00:00 2001
From: Jaeyoung-Lim <jalim@ethz.ch>
Date: Fri, 9 Apr 2021 23:33:48 +0200
Subject: [PATCH] Fixedwing related plugins

This commit merges in fixedwing related plugins
---
 rotors_gazebo_plugins/CMakeLists.txt          |  21 +
 .../gazebo_aerodynamics_plugin.h              | 341 +++++++
 .../gazebo_propulsion_plugin.h                | 251 +++++
 .../gazebo_wind_beta_plugin.cpp               | 863 ++++++++++++++++++
 .../gazebo_wind_beta_plugin.h                 | 303 ++++++
 .../rotors_gazebo_plugins/uav_parameters.h    | 413 +++++++++
 rotors_gazebo_plugins/msgs/ArrowMarker.proto  |  20 +
 .../msgs/ConnectGazeboToRosTopic.proto        |   2 +
 .../msgs/PropulsionSlipstream.proto           |  19 +
 rotors_gazebo_plugins/msgs/VisVector.proto    |  20 +
 .../msgs/VisVectorArray.proto                 |  14 +
 .../msgs/WindSpeedBeta.proto                  |  13 +
 rotors_gazebo_plugins/msgs/matrix3d.proto     |  15 +
 .../src/gazebo_aerodynamics_plugin.cpp        | 822 +++++++++++++++++
 .../src/gazebo_propulsion_plugin.cpp          | 624 +++++++++++++
 .../src/gazebo_wind_beta_plugin.cpp           | 863 ++++++++++++++++++
 16 files changed, 4604 insertions(+)
 create mode 100644 rotors_gazebo_plugins/include/rotors_gazebo_plugins/gazebo_aerodynamics_plugin.h
 create mode 100644 rotors_gazebo_plugins/include/rotors_gazebo_plugins/gazebo_propulsion_plugin.h
 create mode 100644 rotors_gazebo_plugins/include/rotors_gazebo_plugins/gazebo_wind_beta_plugin.cpp
 create mode 100644 rotors_gazebo_plugins/include/rotors_gazebo_plugins/gazebo_wind_beta_plugin.h
 create mode 100644 rotors_gazebo_plugins/include/rotors_gazebo_plugins/uav_parameters.h
 create mode 100644 rotors_gazebo_plugins/msgs/ArrowMarker.proto
 create mode 100644 rotors_gazebo_plugins/msgs/PropulsionSlipstream.proto
 create mode 100644 rotors_gazebo_plugins/msgs/VisVector.proto
 create mode 100644 rotors_gazebo_plugins/msgs/VisVectorArray.proto
 create mode 100644 rotors_gazebo_plugins/msgs/WindSpeedBeta.proto
 create mode 100644 rotors_gazebo_plugins/msgs/matrix3d.proto
 create mode 100644 rotors_gazebo_plugins/src/gazebo_aerodynamics_plugin.cpp
 create mode 100644 rotors_gazebo_plugins/src/gazebo_propulsion_plugin.cpp
 create mode 100644 rotors_gazebo_plugins/src/gazebo_wind_beta_plugin.cpp

diff --git a/rotors_gazebo_plugins/CMakeLists.txt b/rotors_gazebo_plugins/CMakeLists.txt
index bdf6d8123..81e463445 100644
--- a/rotors_gazebo_plugins/CMakeLists.txt
+++ b/rotors_gazebo_plugins/CMakeLists.txt
@@ -301,6 +301,27 @@ if (NOT NO_ROS)
 endif()
 list(APPEND targets_to_install rotors_gazebo_fw_dynamics_plugin)
 
+add_library(rotors_gazebo_aerodynamics_plugin SHARED src/gazebo_aerodynamics_plugin.cpp)
+target_link_libraries(rotors_gazebo_aerodynamics_plugin ${target_linking_LIBRARIES}  ${YamlCpp_LIBRARIES})
+if (NOT NO_ROS)
+  add_dependencies(rotors_gazebo_aerodynamics_plugin ${catkin_EXPORTED_TARGETS})
+endif()
+list(APPEND targets_to_install rotors_gazebo_aerodynamics_plugin)
+
+add_library(rotors_gazebo_wind_beta_plugin SHARED src/gazebo_wind_beta_plugin.cpp)
+target_link_libraries(rotors_gazebo_wind_beta_plugin ${target_linking_LIBRARIES}  ${YamlCpp_LIBRARIES})
+if (NOT NO_ROS)
+  add_dependencies(rotors_gazebo_wind_beta_plugin ${catkin_EXPORTED_TARGETS})
+endif()
+list(APPEND targets_to_install rotors_gazebo_wind_beta_plugin)
+
+add_library(rotors_gazebo_propulsion_plugin SHARED src/gazebo_propulsion_plugin.cpp)
+target_link_libraries(rotors_gazebo_propulsion_plugin ${target_linking_LIBRARIES}  ${YamlCpp_LIBRARIES})
+if (NOT NO_ROS)
+  add_dependencies(rotors_gazebo_propulsion_plugin ${catkin_EXPORTED_TARGETS})
+endif()
+list(APPEND targets_to_install rotors_gazebo_propulsion_plugin)
+
 #========================================= GPS PLUGIN ===========================================//
 add_library(rotors_gazebo_gps_plugin SHARED src/gazebo_gps_plugin.cpp)
 target_link_libraries(rotors_gazebo_gps_plugin ${target_linking_LIBRARIES} )
diff --git a/rotors_gazebo_plugins/include/rotors_gazebo_plugins/gazebo_aerodynamics_plugin.h b/rotors_gazebo_plugins/include/rotors_gazebo_plugins/gazebo_aerodynamics_plugin.h
new file mode 100644
index 000000000..bc253fcbe
--- /dev/null
+++ b/rotors_gazebo_plugins/include/rotors_gazebo_plugins/gazebo_aerodynamics_plugin.h
@@ -0,0 +1,341 @@
+/*
+ * Copyright (C) 2014-2016 Open Source Robotics Foundation
+ *
+ * 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.
+ *
+ *
+ * Modifications by David Rohr, drohr@student.ethz.ch
+ *
+*/
+
+#ifndef _GAZEBO_LIFT_DRAG_PLUGIN_HH_
+#define _GAZEBO_LIFT_DRAG_PLUGIN_HH_
+
+#include <mutex>
+
+#include <string>
+#include <vector>
+
+#include "gazebo/common/Plugin.hh"
+#include "gazebo/physics/physics.hh"
+#include "gazebo/transport/TransportTypes.hh"
+#include <ignition/math.hh>
+
+#include "common.h"
+#include "uav_parameters.h"
+
+#include <math.h>
+#include <Eigen/Eigen>
+#include "gazebo/msgs/msgs.hh"
+#include <stdio.h>
+#include <boost/bind.hpp>
+#include <gazebo/gazebo.hh>
+#include <gazebo/common/common.hh>
+#include "PropulsionSlipstream.pb.h"
+#include "VisVectorArray.pb.h"
+#include "WindSpeedBeta.pb.h"
+#include "ConnectGazeboToRosTopic.pb.h"
+#include <Float32.pb.h>
+#include <iostream>
+#include <fstream>
+
+#include <algorithm>
+
+#include "gazebo/common/Assert.hh"
+#include "gazebo/sensors/SensorManager.hh"
+#include "gazebo/transport/transport.hh"
+#include <iomanip>
+
+#define AIRFOIL_N_VIS_VEC 6
+#define BODY_N_VIS_VEC 2
+
+namespace gazebo
+{
+
+typedef ignition::math::Vector3d V3D;
+typedef ignition::math::Matrix3<double> M3D;
+
+typedef const boost::shared_ptr<const gz_visualization_msgs::VisVectorArray> GzVisVectorArrayMsgPtr;
+typedef const boost::shared_ptr<const gz_mav_msgs::PropulsionSlipstream> PropulsionSlipstreamPtr;
+typedef const boost::shared_ptr<const gz_mav_msgs::WindSpeedBeta> WindPtr;
+typedef const boost::shared_ptr<const gz_std_msgs::Float32> Float32Ptr;
+static const std::string kDefaultPropulsionSlipstreamSubTopic = "/propulsion_slipstream";
+static constexpr double kDefaultRhoAir = 1.2041; // air density 1.2041 kg/m³ (dry, @20 °C, 101.325 kPa)
+
+
+class GazeboAerodynamics : public ModelPlugin
+{
+    /// \brief Constructor.
+public:
+    GazeboAerodynamics():
+        ModelPlugin(){gzdbg<<"gazebo_aerodynamics constructed"<<std::endl;}
+    ~GazeboAerodynamics();
+
+protected:
+    void Load(physics::ModelPtr _model, sdf::ElementPtr _sdf);
+    void OnUpdate();
+
+private:
+    std::string namespace_;
+    transport::NodePtr node_handle_;
+    physics::WorldPtr world;
+    physics::ModelPtr model;
+    physics::LinkPtr link;
+    event::ConnectionPtr updateConnection;
+
+    double rho_ = kDefaultRhoAir; // air density 1.2041 kg/m³ (dry, @20 °C, 101.325 kPa)
+
+    struct ControlSurface {
+        ControlSurface():
+            cs_ref(0.0){}
+
+        physics::JointPtr control_joint = nullptr;
+        transport::SubscriberPtr control_ref_sub = nullptr;
+        std::string cs_ref_topic;
+        bool from_topic = false;
+        std::atomic<double> cs_ref;
+        V3D control_joint_rad_to_cl = V3D(0,0,0);   // dC_L/dCS slope, [1/rad]
+        V3D control_joint_rad_to_cd = V3D(0,0,0);   // dC_D/dCS slope, [1/rad]
+        V3D control_joint_rad_to_cm = V3D(0,0,0);   // dC_M/dCS slope, [1/rad]
+        double d_aoa_b_d_delta_cs = 0;                // dAlpha_break/dCs slope [rad/rad]
+        void Callback(Float32Ptr& reference){
+            cs_ref = (double)reference->data();
+        }
+    };
+
+    struct Wind {
+        Wind(){}
+
+        transport::SubscriberPtr wind_sub_ = nullptr;
+        std::mutex wind_lock;
+        std::string wind_topic;
+
+        V3D pos_ned = V3D(0,0,0);
+        V3D wind_ned = V3D(0,0,0);
+        M3D wind_grad_ned = M3D(0,0,0,0,0,0,0,0,0);
+
+        void Callback(WindPtr& wind){
+            std::unique_lock<std::mutex> lock(wind_lock);
+
+            pos_ned = V3D(wind->pos_ned().x(),
+                          wind->pos_ned().y(),
+                          wind->pos_ned().z());
+
+            wind_ned = V3D(wind->wind_ned().x(),
+                           wind->wind_ned().y(),
+                           wind->wind_ned().z());
+
+            wind_grad_ned = M3D(wind->wind_grad_ned().xx(),
+                                wind->wind_grad_ned().xy(),
+                                wind->wind_grad_ned().xz(),
+                                wind->wind_grad_ned().yx(),
+                                wind->wind_grad_ned().yy(),
+                                wind->wind_grad_ned().yz(),
+                                wind->wind_grad_ned().zx(),
+                                wind->wind_grad_ned().zy(),
+                                wind->wind_grad_ned().zz());
+            lock.unlock();
+        }
+
+        V3D GetWind(V3D p_cp){
+            std::unique_lock<std::mutex> lock(wind_lock);   //necessary? atomic V3D?
+            V3D wind_local = wind_ned + wind_grad_ned*(p_cp-pos_ned);
+            //V3D wind_local = wind_grad_ned*(p_cp-pos_ned);
+            lock.unlock();
+            return wind_local;
+        }
+    };
+
+    struct Slipstream {
+        Slipstream(){}
+
+        transport::SubscriberPtr propulsion_slipstream_sub_ = nullptr;
+        std::mutex slipstream_lock;
+        std::string slpstr_topic;
+
+        V3D p_rot = V3D(0,0,0);
+        V3D d_wake = V3D(0,0,0);
+        V3D d_wake_e = V3D(0,0,0);
+        V3D v_ind_d = V3D(0,0,0);
+        V3D v_ind_e = V3D(0,0,0);
+
+        double r_rot = 0;
+
+        void Callback(PropulsionSlipstreamPtr& propulsion_slipstream){
+            std::unique_lock<std::mutex> lock(slipstream_lock);
+            // position of rotordisk center wrt world, expressed in world frame [m]
+            p_rot = V3D(propulsion_slipstream->rotor_pos().x(),
+                        propulsion_slipstream->rotor_pos().y(),
+                        propulsion_slipstream->rotor_pos().z());
+
+            // wake direction expressed in world frame [-]
+            d_wake = V3D(propulsion_slipstream->wake_dir().x(),
+                         propulsion_slipstream->wake_dir().y(),
+                         propulsion_slipstream->wake_dir().z());
+            d_wake_e = d_wake.Normalized();
+
+            // induced velocity at rotordisk, expressed in world frame [m/s]
+            v_ind_d = V3D(propulsion_slipstream->ind_vel_disk().x(),
+                          propulsion_slipstream->ind_vel_disk().y(),
+                          propulsion_slipstream->ind_vel_disk().z());
+
+            // induced velocity at end of wake (Note: not necessarily 0!)
+            v_ind_e = V3D(propulsion_slipstream->ind_vel_end().x(),
+                          propulsion_slipstream->ind_vel_end().y(),
+                          propulsion_slipstream->ind_vel_end().z());
+
+            // propeller/wake diameter
+            r_rot = propulsion_slipstream->prop_diam()/2;
+            lock.unlock();
+        }
+
+        V3D GetIndVel(V3D p_cp){
+
+            V3D v_ind; // induced velocity at cp (e.g. due to slipstream)
+            std::unique_lock<std::mutex> lock(slipstream_lock);
+            V3D p_r2cp_ = p_cp - p_rot;
+            double off_a_ = d_wake_e.Dot(p_r2cp_);                 // axial distance in wake (d1 in report)
+            double off_p_ = (off_a_*d_wake_e-p_r2cp_).Length();    // radial distance to wake centerline (d2 in report)
+
+
+            if(off_a_>0 && d_wake.Length()>off_a_){
+                // if in zone of slipstream influence
+                double k_a_ = (d_wake.Length()-off_a_)/d_wake.Length();    // axial direction interpolation weight
+                double r_rot_exp = (2-1*k_a_)*r_rot;
+                double k_p_ = 1-pow((off_p_/r_rot_exp),4);
+                k_p_ = ignition::math::clamp(k_p_,0.0,1.0);                // radial distance downscaling
+                v_ind = k_p_*(k_a_*v_ind_d+(1-k_a_)*v_ind_e);   // induced velocity at airfoil segment cp
+            }
+
+            lock.unlock();
+            return v_ind;
+        }
+    };
+
+    struct Segment {
+        Segment(){}
+
+        physics::LinkPtr ref_link = nullptr;  // reference link for force/torque calculation
+        physics::LinkPtr act_link = nullptr;  // link to apply force/torque to
+
+        // To include hyteresis in future implementation, currently not used
+        double alpha_prev = 0;
+        double alpha_dot = 0;
+        bool cl_hist_up = true;
+
+        AerodynamicParameters aero_params;
+
+        V3D cp = V3D(0,0,0);    // Center of pressure wrt link frame, expressed in link frame
+        V3D fwd = V3D(1,0,0);
+        V3D upwd = V3D(0,0,1);
+        double seg_area;
+
+        double seg_chord;
+        double ellp_mult;
+
+        ControlSurface * cs;
+        int n_cs = 0;
+
+        /// \brief Quantities to model propeller/rotor wake/slipstream
+
+        Slipstream * slpstr;
+        V3D v_ind_cp; // induced velocity at cp (e.g. due to slipstream)
+        int n_slpstr = 0;
+
+        Wind * wind;
+        V3D wind_cp;
+        int n_wind = 0;
+
+        /// \brief For visualization in rviz
+        std::string vector_vis_array_topic;
+        gazebo::transport::PublisherPtr vector_vis_array_pub;
+        gz_visualization_msgs::VisVectorArray vector_vis_array_msg;
+        std::array<gz_visualization_msgs::ArrowMarker*, AIRFOIL_N_VIS_VEC> vec_vis;
+
+        void UpdateIndVel(V3D w_cp) {
+            v_ind_cp = V3D(0,0,0);
+            for(int j=0; j<n_slpstr; j++){
+                v_ind_cp += slpstr[j].GetIndVel(w_cp);
+            }
+        }
+
+        void UpdateWind(V3D w_cp) {
+            wind_cp = V3D(0,0,0);
+            for(int j=0; j<n_wind; j++){
+                wind_cp += wind[j].GetWind(w_cp);
+            }
+        }
+
+    };
+
+    Segment * segments_;
+    int n_seg_ = 0;
+
+    struct Body {
+
+        Body():
+            a_fus_xx(0),
+            a_fus_yy(0),
+            a_fus_zz(0),
+            cd_cyl_ax(0.82),
+            cd_cyl_lat(1.17),
+            cp(0,0,0),
+            fwd(1,0,0),
+            upwd(0,0,1){}
+
+        physics::LinkPtr ref_link = nullptr;  // reference link for force/torque calculation
+        physics::LinkPtr act_link = nullptr;  // link to apply force/torque to
+
+        double a_fus_xx;                      // forward-projected area of fuselage, [m^2]
+        double a_fus_yy;                      // side-projected area of fuselage, [m^2]
+        double a_fus_zz;                      // down-projected area of fuselage, [m^2]
+        double cd_cyl_ax;                     // drag coefficient of long cylinder in axial flow, [-]
+        double cd_cyl_lat;                    // drag coefficient of cylinder in lateral flow, [-]
+
+        V3D cp;
+        V3D fwd;
+        V3D upwd;
+
+        Wind * wind;
+        V3D wind_cp;
+        int n_wind = 0;
+
+        /// \brief For visualization in rviz
+        std::string vector_vis_array_topic;
+        gazebo::transport::PublisherPtr vector_vis_array_pub;
+        gz_visualization_msgs::VisVectorArray vector_vis_array_msg;
+        std::array<gz_visualization_msgs::ArrowMarker*, BODY_N_VIS_VEC> vec_vis;
+
+        void UpdateWind(V3D w_cp) {
+            wind_cp = V3D(0,0,0);
+            for(int j=0; j<n_wind; j++){
+                wind_cp += wind[j].GetWind(w_cp);
+            }
+        }
+    };
+
+    Body * bodies_;
+    int n_bdy_ = 0;
+
+    std::string vector_vis_array_topic_;
+    gz_visualization_msgs::VisVectorArray vector_vis_array_;
+    gazebo::transport::PublisherPtr vector_vis_array_pub_;
+
+    common::Time last_time_;
+
+    bool pubs_and_subs_created_ = false;
+
+};
+
+}
+#endif
diff --git a/rotors_gazebo_plugins/include/rotors_gazebo_plugins/gazebo_propulsion_plugin.h b/rotors_gazebo_plugins/include/rotors_gazebo_plugins/gazebo_propulsion_plugin.h
new file mode 100644
index 000000000..3e6db1111
--- /dev/null
+++ b/rotors_gazebo_plugins/include/rotors_gazebo_plugins/gazebo_propulsion_plugin.h
@@ -0,0 +1,251 @@
+/*
+ * Copyright 2015 Fadri Furrer, ASL, ETH Zurich, Switzerland
+ * Copyright 2015 Michael Burri, ASL, ETH Zurich, Switzerland
+ * Copyright 2015 Mina Kamel, ASL, ETH Zurich, Switzerland
+ * Copyright 2015 Janosch Nikolic, ASL, ETH Zurich, Switzerland
+ * Copyright 2015 Markus Achtelik, ASL, ETH Zurich, Switzerland
+ *
+ * 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.
+ *
+ * Modifications by David Rohr, drohr@student.ethz.ch
+ *
+ */
+
+#include <stdio.h>
+#include <iostream>
+#include <fstream>
+
+#include <ignition/math.hh>
+#include <math.h>
+#include <boost/bind.hpp>
+#include <Eigen/Eigen>
+
+#include <gazebo/gazebo.hh>
+#include <gazebo/physics/physics.hh>
+#include <gazebo/common/common.hh>
+#include <gazebo/common/Plugin.hh>
+
+#include "CommandMotorSpeed.pb.h"
+#include "gazebo/transport/transport.hh"
+#include "gazebo/msgs/msgs.hh"
+#include "PropulsionSlipstream.pb.h"
+#include "VisVectorArray.pb.h"
+#include "WindSpeedBeta.pb.h"
+#include "ConnectGazeboToRosTopic.pb.h"
+#include "Float32.pb.h"
+#include "vector2d.pb.h"
+
+#include "common.h"
+#include "uav_parameters.h"
+
+namespace gazebo {
+
+typedef const boost::shared_ptr<const gz_visualization_msgs::VisVectorArray> GzVisVectorArrayMsgPtr;
+typedef const boost::shared_ptr<const gz_std_msgs::Float32> GzFloat32MsgPtr;
+typedef const boost::shared_ptr<const gz_mav_msgs::WindSpeedBeta> WindPtr;
+typedef ignition::math::Vector3d V3D;
+typedef ignition::math::Matrix3<double> M3D;
+
+// Default values
+static constexpr double kDefaulMaxRotVelocity = 838.0;
+static constexpr double kDefaultRhoAir = 1.2041; // air density 1.2041 kg/m³ (dry, @20 °C, 101.325 kPa)
+
+class GazeboPropulsion : public ModelPlugin{
+public:
+    GazeboPropulsion():
+        ModelPlugin(){}
+
+    ~GazeboPropulsion();
+
+protected:
+    void Load(physics::ModelPtr _model, sdf::ElementPtr _sdf);
+    //void OnUpdate(const common::UpdateInfo & _info);
+    void OnUpdate();
+
+private:
+    std::string namespace_;
+    transport::NodePtr node_handle_;
+    physics::WorldPtr world_;
+    physics::ModelPtr model_;
+    event::ConnectionPtr update_connection_;
+
+    int update_counter_ = 0;
+    double sampling_time_ = 0.0;    // simulation time-step [s]
+    double prev_sim_time_ = 0.0;    // simulation time [s]
+
+    struct Wind {
+        Wind(){}
+
+        transport::SubscriberPtr wind_sub_ = nullptr;
+        std::mutex wind_lock;
+        std::string wind_topic;
+
+        V3D pos_ned = V3D(0,0,0);
+        V3D wind_ned = V3D(0,0,0);
+        M3D wind_grad_ned = M3D(0,0,0,0,0,0,0,0,0);
+
+        void Callback(WindPtr& wind){
+            std::unique_lock<std::mutex> lock(wind_lock);
+
+            pos_ned = V3D(wind->pos_ned().x(),
+                          wind->pos_ned().y(),
+                          wind->pos_ned().z());
+
+            wind_ned = V3D(wind->wind_ned().x(),
+                           wind->wind_ned().y(),
+                           wind->wind_ned().z());
+
+            wind_grad_ned = M3D(wind->wind_grad_ned().xx(),
+                                wind->wind_grad_ned().xy(),
+                                wind->wind_grad_ned().xz(),
+                                wind->wind_grad_ned().yx(),
+                                wind->wind_grad_ned().yy(),
+                                wind->wind_grad_ned().yz(),
+                                wind->wind_grad_ned().zx(),
+                                wind->wind_grad_ned().zy(),
+                                wind->wind_grad_ned().zz());
+            lock.unlock();
+        }
+
+        V3D GetWind(V3D p_cp){
+            std::unique_lock<std::mutex> lock(wind_lock);   //necessary? atomic V3D?
+            V3D wind_local = wind_ned + wind_grad_ned*(p_cp-pos_ned);
+            //V3D wind_local = wind_grad_ned*(p_cp-pos_ned);
+            lock.unlock();
+            return wind_local;
+        }
+    };
+
+    struct Propeller {
+        Propeller(){}
+
+        PropellerParameters prop_params;
+
+        physics::LinkPtr ref_link = nullptr;  // reference link for force/torque calculation
+        physics::LinkPtr act_link = nullptr;  // link to apply force/torque to
+
+        ignition::math::Vector3d p_joint{1,0,0};    // propeller joint, pos. rot. dir., expr. in parent link
+        ignition::math::Vector3d p_cp{0,0,0};       // propeller hub position w.r.t parent link, expr. in parent link
+        int turning_direction = 1;                  // 1: if thrust and rot. vect in same dir, otherwise -1
+
+        V3D H_Omega_Prev_{0,0,0};
+
+        M3D inertia;           // propeller (disk) inertia tensor, expressed in parent frame
+        double omega = 0;      // propeller angular speed (wrt parent link) [rad/s]
+        double omega_dot = 0;  // propeller angular acc (wrt parent link) [rad/s²]
+        double omega_ref = 0;  // propeller angular ref speed (wrt parent link) [rad/s]
+        double omega_dead = 0; // ESC dead-zone. If omega_ref below this value prop wont spin [rad/s]
+        double omega_max = 1e5;//
+        double tau_p = 0.1;    // prop/motor time constant rising [s]
+        double tau_n = 0.3;    // prop/motor time constant falling [s]
+        double tau_su = 0.1;     // prop/motor time constant spool up [s]
+        double dt = 0.0;       // discrete time step for motor simulation [s]
+
+        transport::SubscriberPtr omega_ref_sub = nullptr;
+        std::string omega_ref_subtopic;
+
+        //ignition::math::Vector3d cp{0,0,0};
+
+        transport::PublisherPtr prop_slpstr_pub = nullptr;
+        std::string prop_slpstr_pubtopic;
+
+        transport::PublisherPtr speed_pub = nullptr;
+
+        std::array<gz_visualization_msgs::ArrowMarker*,7> vec_vis;
+        gz_visualization_msgs::ArrowMarker* wind_vis;
+        std::string vector_vis_array_topic;
+        gz_visualization_msgs::VisVectorArray vector_vis_array_msg;
+        gazebo::transport::PublisherPtr vector_vis_array_pub;
+
+        Wind * wind;
+        V3D wind_cp;
+        int n_wind = 0;
+
+        void PropSpeedCallback(GzFloat32MsgPtr& ref){
+            omega_ref = (double)ref->data();
+        }
+
+        void MotorDyn(){
+
+            // constrain speed reference with dead-zone
+            if (omega_ref<omega_dead)
+                omega_ref = 0.0;
+
+            if (omega_ref>omega_max)
+                omega_ref=omega_max;
+
+            if (tau_p>0.0 && tau_n>0.0 && tau_su>0.0 && dt>0.0) {
+
+                if (omega<omega_dead && omega_ref>=omega_dead) {
+                    // throttle up from within deadzone
+
+                    // Model 1: 1st order
+                    omega = omega + std::min(dt, tau_su)/tau_su*(omega_ref-omega);
+                    omega_dot = (omega_ref-omega)*std::min(dt, tau_su)/(dt*tau_su);
+
+                    // Model 2: slew-rate:
+                    /* double slew_dead = omega_dead/0.15/tau_su; // approx. fastest rise
+                    omega = ignition::math::clamp(omega + slew_dead*dt, 0.0, omega_ref);
+                    omega_dot = slew_dead; */
+
+                } else {
+                    if (omega_ref>=omega){
+                        omega = omega + std::min(dt, tau_p)/tau_p*(omega_ref-omega);
+                        omega_dot = (omega_ref-omega)*std::min(dt, tau_p)/(dt*tau_p);
+
+                    } else {
+                        omega = omega + std::min(dt, tau_n)/tau_n*(omega_ref-omega);
+                        omega_dot = (omega_ref-omega)*std::min(dt, tau_n)/(dt*tau_n);
+                    }
+                }
+
+                gazebo::msgs::Vector2d speed_msg;
+                speed_msg.set_x(omega);
+                speed_msg.set_y(omega_dot);
+
+                if (speed_pub)
+                    speed_pub->Publish(speed_msg);
+
+            } else {
+                gzerr<<"simulation time-step and/or motor time-constant equals zero\n";
+            }
+
+            if(isnan(omega)||isinf(omega)){
+                omega = 0.0;
+                gzerr<<"bad omega detected, setting to zero!!\n";
+            }
+        }
+
+        void UpdateWind(V3D w_cp){
+            wind_cp = V3D(0,0,0);
+            for(int j=0; j<n_wind; j++){
+                wind_cp += wind[j].GetWind(w_cp);
+            }
+        }
+    };
+
+    Propeller* propellers_;
+    int num_props_ = 0;
+
+    double max_rot_velocity_ = kDefaulMaxRotVelocity;   // [rad/s]
+    double rho_air_ = kDefaultRhoAir;                   // [kg/m^3]
+
+    gz_mav_msgs::PropulsionSlipstream propulsion_slipstream_msg_;
+    bool pubs_and_subs_created_ = false;
+
+    int Sgn(double val) {
+        return (int)(0.0 < val) - (int)(val < 0.0);
+    }
+
+};
+}
diff --git a/rotors_gazebo_plugins/include/rotors_gazebo_plugins/gazebo_wind_beta_plugin.cpp b/rotors_gazebo_plugins/include/rotors_gazebo_plugins/gazebo_wind_beta_plugin.cpp
new file mode 100644
index 000000000..e487ba2d6
--- /dev/null
+++ b/rotors_gazebo_plugins/include/rotors_gazebo_plugins/gazebo_wind_beta_plugin.cpp
@@ -0,0 +1,863 @@
+/*
+ * Copyright 2015 Fadri Furrer, ASL, ETH Zurich, Switzerland
+ * Copyright 2015 Michael Burri, ASL, ETH Zurich, Switzerland
+ * Copyright 2015 Mina Kamel, ASL, ETH Zurich, Switzerland
+ * Copyright 2015 Janosch Nikolic, ASL, ETH Zurich, Switzerland
+ * Copyright 2015 Markus Achtelik, ASL, ETH Zurich, Switzerland
+ * Copyright 2016 Geoffrey Hunter <gbmhunter@gmail.com>
+ *
+ * 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 "rotors_gazebo_plugins/gazebo_wind_beta_plugin.h"
+
+#include <fstream>
+#include <math.h>
+
+#include "ConnectGazeboToRosTopic.pb.h"
+
+namespace gazebo {
+
+GazeboWindBetaPlugin::~GazeboWindBetaPlugin() {
+    delete[] wind_queries_;
+    delete[] wind_layers_;
+    delete[] layer_index_alt_incr_;
+
+    for (int iix=0; iix<nk_x_; iix++) {
+        for (int iiy=0; iiy<nk_y_; iiy++) {
+            delete[] C_ij_[iix][iiy];
+            delete[] rand_phase_[iix][iiy];
+        }
+        delete[] C_ij_[iix];
+        delete[] rand_phase_[iix];
+    }
+    delete[] rand_phase_;
+    delete[] C_ij_;
+}
+
+void GazeboWindBetaPlugin::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf) {
+    if (kPrintOnPluginLoad) {
+        gzdbg << __FUNCTION__ << "() called." << std::endl;
+    }
+
+    // Store the pointer to the model.
+    model_ = _model;
+    world_ = model_->GetWorld();
+
+    //==============================================//
+    //========== READ IN PARAMS FROM SDF ===========//
+    //==============================================//
+
+    if (_sdf->HasElement("robotNamespace"))
+        namespace_ = _sdf->GetElement("robotNamespace")->Get<std::string>();
+    else
+        gzerr << "[gazebo_wind_beta_plugin] Please specify a robotNamespace.\n";
+
+    // Create Gazebo Node.
+    node_handle_ = gazebo::transport::NodePtr(new transport::Node());
+
+    // Initialize with default namespace (typically /gazebo/default/).
+    node_handle_->Init();
+
+    if (_sdf->HasElement("findWind")) {
+
+        sdf::ElementPtr _sdf_findWind = _sdf->GetElement("findWind");
+        sdf::ElementPtr _sdf_query = _sdf_findWind->GetElement("query");
+
+        while (_sdf_query) {
+            _sdf_query = _sdf_query->GetNextElement("query");
+            ++n_query_;
+        }
+
+        gzdbg<<"Found "<<n_query_<<" points to calculate wind \n";
+        wind_queries_ = new WindQuery [n_query_];
+
+        _sdf_query = _sdf_findWind->GetElement("query");
+
+        for (int i=0; i<n_query_; i++) {
+            if (_sdf_query->HasElement("linkName")) {
+                std::string link_name = _sdf_query->Get<std::string>("linkName");
+                wind_queries_[i].link = model_->GetLink(link_name);
+
+                if (!wind_queries_[i].link) {
+                    gzerr<<"Couldn't find specified link \"" << link_name << "\", won't publish.\n";
+                    wind_queries_[i].sdf_valid = false;
+                }
+
+            } else if (_sdf_query->HasElement("worldPos")) {
+                wind_queries_[i].pos_ned = _sdf_query->Get<ignition::math::Vector3d>("worldPos");
+
+            } else {
+                gzwarn<<"Don't know where to query wind, won't publish. \n";
+                wind_queries_[i].sdf_valid = false;
+            }
+
+            if (_sdf_query->HasElement("windTopic")) {
+                wind_queries_[i].wind_topic = _sdf_query->Get<std::string>("windTopic");
+
+            } else {
+                gzwarn<<"Please specify windTopic, won't publish wind data. \n";
+                wind_queries_[i].sdf_valid = false;
+            }
+
+            getSdfParam<std::string>(_sdf_query, "frameId", wind_queries_[i].frame_id, wind_queries_[i].frame_id);
+
+            _sdf_query = _sdf_query->GetNextElement("query");
+        }
+    }
+
+    if (_sdf->HasElement("windLayer")) {
+
+        sdf::ElementPtr _sdf_windLayer = _sdf->GetElement("windLayer");
+        sdf::ElementPtr _sdf_layer = _sdf_windLayer->GetElement("layer");
+
+        while (_sdf_layer) {
+            _sdf_layer = _sdf_layer->GetNextElement("layer");
+            ++n_layer_;
+        }
+
+        gzdbg<<"Found "<<n_layer_<<" wind layers \n";
+        wind_layers_ = new WindLayer [n_layer_];
+        layer_index_alt_incr_ = new int [n_layer_];
+        _sdf_layer = _sdf_windLayer->GetElement("layer");
+
+        for (int i=0; i<n_layer_; i++) {
+            getSdfParam<double>(_sdf_layer, "altitude", wind_layers_[i].altitude,
+                                wind_layers_[i].altitude);
+            /*
+               getSdfParam<double>(_sdf_layer, "altUpper", wind_layers_[i].alt_upper,
+                                   wind_layers_[i].alt_upper);
+               getSdfParam<bool>(_sdf_layer, "useRelAlt", wind_layers_[i].rel_alt,
+                                 wind_layers_[i].rel_alt);
+               */
+
+            getSdfParam<V3D>(_sdf_layer, "windMeanNED", wind_layers_[i].wind_mean,
+                             wind_layers_[i].wind_mean);
+            /*
+               getSdfParam<double>(_sdf_layer, "windSpeedVariance", wind_layers_[i].wind_speed_variance,
+                                   wind_layers_[i].wind_speed_variance);
+               getSdfParam<bool>(_sdf_layer, "gustSpeedMean", wind_layers_[i].rel_alt,
+                                 wind_layers_[i].rel_alt);
+               getSdfParam<bool>(_sdf_layer, "gustSpeedVariance", wind_layers_[i].rel_alt,
+                                 wind_layers_[i].rel_alt);
+               getSdfParam<bool>(_sdf_layer, "windNedDir", wind_layers_[i].rel_alt,
+                                 wind_layers_[i].rel_alt);
+               getSdfParam<bool>(_sdf_layer, "gustNedDir", wind_layers_[i].rel_alt,
+                                 wind_layers_[i].rel_alt);
+               getSdfParam<bool>(_sdf_layer, "gust", wind_layers_[i].rel_alt,
+                                 wind_layers_[i].rel_alt);
+               */
+
+            _sdf_layer = _sdf_layer->GetNextElement("layer");
+        }
+
+        // sort layers, ascending altitude
+        std::sort(wind_layers_,wind_layers_+n_layer_,GazeboWindBetaPlugin::SortByAltitude);
+    }
+
+    if (_sdf->HasElement("lambdaMin"))
+        lambda_min_ = _sdf->Get<double>("lambdaMin");
+
+    if (_sdf->HasElement("lengthScale"))
+        L_ = _sdf->Get<double>("lengthScale");
+
+    if (_sdf->HasElement("fourierNNN")) {
+        ignition::math::Vector3i fourierNNN = _sdf->Get<ignition::math::Vector3i>("fourierNNN");
+        nk_x_ = std::max(1,fourierNNN.X());
+        nk_y_ = std::max(1,fourierNNN.Y());
+        nk_z_ = std::max(1,fourierNNN.Z());
+
+    } else {
+        nk_x_ = 15;
+        nk_y_ = 15;
+        nk_z_ = 15;
+    }
+
+    // ensure odd number of components
+    if(nk_x_%2==0)
+        nk_x_+=1;
+
+    if(nk_y_%2==0)
+        nk_y_+=1;
+
+    if(nk_z_%2==0)
+        nk_z_+=1;
+
+    if (_sdf->HasElement("sigma"))
+        sigma_ = _sdf->Get<double>("sigma");
+
+    if (_sdf->HasElement("trnspVelTurbNED"))
+        trnsp_vel_turb_ = _sdf->Get<V3D>("trnspVelTurbNED");
+
+    if (_sdf->HasElement("customWindFieldPath")) {
+        gzdbg << "[gazebo_wind_plugin] Using custom wind field from text file.\n";
+        // Get the wind field text file path, read it and save data.
+        std::string custom_wind_field_path;
+        getSdfParam<std::string>(_sdf, "customWindFieldPath", custom_wind_field_path,
+                                 custom_wind_field_path);
+
+        getSdfParam<double>(_sdf, "windFieldXOffset", wf_offset.X(), wf_offset.X());
+        getSdfParam<double>(_sdf, "windFieldYOffset", wf_offset.Y(), wf_offset.Y());
+        getSdfParam<double>(_sdf, "windFieldZOffset", wf_offset.Z(), wf_offset.Z());
+
+        ReadCustomWindFieldCSV(custom_wind_field_path);
+        ProcessCustomWindField();
+    }
+
+    // Set up Fourier Simulation
+    std::random_device rd;
+    std::mt19937 gen(rd());
+    //std::uniform_real_distribution<> r_phase(0.0, 2*M_PI);
+    std::normal_distribution<> re(0,1.0/sqrt(2));   // complex variance = var(re)+var(im) needs= 1
+    std::normal_distribution<> im(0,1.0/sqrt(2));
+
+    C_ij_ = new M3C** [nk_x_];
+    rand_phase_ = new V3C** [nk_x_];
+
+    for (int iix=0; iix<nk_x_; iix++) {
+        C_ij_[iix] = new M3C* [nk_y_];
+        rand_phase_[iix] = new V3C* [nk_y_];
+
+        for (int iiy=0; iiy<nk_y_; iiy++) {
+            C_ij_[iix][iiy] = new M3C [nk_z_];
+            rand_phase_[iix][iiy] = new V3C [nk_z_];
+        }
+    }
+
+    dk_x_ = 2*M_PI/lambda_min_/nk_x_;
+    dk_y_ = 2*M_PI/lambda_min_/nk_y_;
+    dk_z_ = 2*M_PI/lambda_min_/nk_z_;
+
+    for (int iix=0; iix<nk_x_; iix++) {
+        for (int iiy=0; iiy<nk_y_; iiy++) {
+            for (int iiz=0; iiz<nk_z_; iiz++) {
+                V3D k((iix-(nk_x_-1)/2)*dk_x_, (iiy-(nk_y_-1)/2)*dk_y_, (iiz-(nk_z_-1)/2)*dk_z_);
+
+                /*
+                rand_phase_[iix][iiy][iiz].Set(std::exp((std::complex<double>)1i*r_phase(gen)),
+                                               std::exp((std::complex<double>)1i*r_phase(gen)),
+                                               std::exp((std::complex<double>)1i*r_phase(gen)));
+                */
+                rand_phase_[iix][iiy][iiz].Set((std::complex<double>)re(gen) + (std::complex<double>)1i*im(gen),
+                                               (std::complex<double>)re(gen) + (std::complex<double>)1i*im(gen),
+                                               (std::complex<double>)re(gen) + (std::complex<double>)1i*im(gen));
+
+                // only use k's inside spherical volume, otherwise set contribution to zero
+                if (k.Length()>0 && k.Length()<M_PI/lambda_min_) {
+                    GazeboWindBetaPlugin::SpectralTensorIsoIncDec(C_ij_[iix][iiy][iiz], k, L_, sigma_);
+                    C_ij_[iix][iiy][iiz] = C_ij_[iix][iiy][iiz]*sqrt(dk_x_*dk_y_*dk_z_);
+
+                } else {
+                    C_ij_[iix][iiy][iiz].Set(0,0,0,0,0,0,0,0,0);
+                }
+            }
+        }
+    }
+
+    /*
+     gzdbg<<"C_ij_[2][3][4]: \n"; PrintMatrix(C_ij_[2][3][4]); gzdbg<<"\n";
+     gzdbg<<"C_ij_[8][7][13]: \n"; PrintMatrix(C_ij_[8][7][13]); gzdbg<<"\n";
+     gzdbg<<"C_ij_[0][1][13]: \n"; PrintMatrix(C_ij_[0][1][13]); gzdbg<<"\n";
+     gzdbg<<"dk_x_: "<<dk_x_<<" dk_y_: "<<dk_y_<<" dk_z_: "<<dk_z_<<"\n";
+     gzdbg<<"lambdaMin: "<<lambda_min_<<" lengthScale: "<<L_<<" sigma: "<<sigma_<<"\n";
+     */
+
+    // Listen to the update event. This event is broadcast every
+    // simulation iteration.
+    update_connection_ = event::Events::ConnectWorldUpdateBegin(boost::bind(&GazeboWindBetaPlugin::OnUpdate, this, _1));
+}
+
+void GazeboWindBetaPlugin::PrintMatrix(M3C M) {
+
+    gzdbg<<std::real(M(0,0))<<" "<<std::real(M(0,1))<<" "<<std::real(M(0,2))<<"\n";
+    gzdbg<<std::real(M(1,0))<<" "<<std::real(M(1,1))<<" "<<std::real(M(1,2))<<"\n";
+    gzdbg<<std::real(M(2,0))<<" "<<std::real(M(2,1))<<" "<<std::real(M(2,2))<<"\n";
+}
+
+// This gets called by the world update start event.
+void GazeboWindBetaPlugin::OnUpdate(const common::UpdateInfo& _info) {
+
+    if (!pubs_and_subs_created_) {
+        CreatePubsAndSubs();
+        pubs_and_subs_created_ = true;
+    }
+
+    for (int iiq=0; iiq<n_query_; iiq++) {
+
+        ignition::math::Pose3d link_pose = wind_queries_[iiq].link->WorldPose();
+        V3D pos = link_pose.Pos();
+
+        // wind component from layered model (from xacro)
+        V3D vel_lyr = V3D(0,0,0);
+
+        // wind component from turbulent field
+        V3C vel_turb = V3C(0,0,0);
+        V3C dvel_x_turb = V3C(0,0,0);
+        V3C dvel_y_turb = V3C(0,0,0);
+        V3C dvel_z_turb = V3C(0,0,0);
+
+        // wind component from custom field (from file)
+        V3D vel_cust_field = V3D(0,0,0);
+        V3D dvdx_cust_field = V3D(0,0,0);
+        V3D dvdy_cust_field = V3D(0,0,0);
+        V3D dvdz_cust_field = V3D(0,0,0);
+
+        // Layered mean velocity field from xacro (uniform in x-y)
+        if (n_layer_>=2) {
+            for (int iil=0; iil<n_layer_-1; iil++) {
+                // below lowest specified layer
+                if (pos.Z()<wind_layers_[iil].altitude) {
+                    if (pos.Z()<=0)
+                        vel_lyr = V3D(0,0,0);
+                    else
+                        vel_lyr = wind_layers_[iil].wind_mean*pos.Z()/wind_layers_[iil].altitude;
+                    break;
+                }
+
+                // in range of specified layers
+                if (pos.Z()>=wind_layers_[iil].altitude && pos.Z()<wind_layers_[iil+1].altitude) {
+                    vel_lyr = wind_layers_[iil].wind_mean +
+                            (wind_layers_[iil+1].wind_mean-wind_layers_[iil].wind_mean)*
+                            (pos.Z()-wind_layers_[iil].altitude)/(wind_layers_[iil+1].altitude-wind_layers_[iil].altitude);
+                    break;
+                }
+
+                // on or above top layer
+                if (iil==n_layer_-2)
+                    vel_lyr = wind_layers_[iil+1].wind_mean;
+            }
+
+        } else if (n_layer_==1) {
+            if (pos.Z()<=0)
+                vel_lyr = V3D(0,0,0);
+            else if (pos.Z()<=wind_layers_[0].altitude)
+                vel_lyr = wind_layers_[0].wind_mean*pos.Z()/wind_layers_[0].altitude;
+            else
+                vel_lyr = wind_layers_[0].wind_mean;
+
+        } else {
+            vel_lyr = V3D(0,0,0);
+        }
+
+        // ToDo: more robust trafo NED to world
+        vel_lyr.Y() = -vel_lyr.Y();
+        vel_lyr.Z() = -vel_lyr.Z();
+
+        // Turbulence field (Fourier simulation)
+#if GAZEBO_MAJOR_VERSION >= 9
+        V3D pos_off = -trnsp_vel_turb_*world_->SimTime().Double();
+#else
+        V3D pos_off = -trnsp_vel_turb_*world_->GetSimTime().Double();
+#endif
+
+        V3D pos_q = pos + pos_off;
+        V3D pos_q_dx = pos + pos_off + V3D(1,0,0);
+        V3D pos_q_dy = pos + pos_off + V3D(0,1,0);
+        V3D pos_q_dz = pos + pos_off + V3D(0,0,1);
+
+        for (int iix=0; iix<nk_x_; iix++) {
+            for (int iiy=0; iiy<nk_y_; iiy++) {
+                for (int iiz=0; iiz<nk_z_; iiz++) {
+                    V3D k((iix-(nk_x_-1)/2)*dk_x_, (iiy-(nk_y_-1)/2)*dk_y_, (iiz-(nk_z_-1)/2)*dk_z_);
+                    V3C arg = C_ij_[iix][iiy][iiz]*rand_phase_[iix][iiy][iiz];
+                    vel_turb = vel_turb + std::exp((std::complex<double>)-1i*k.Dot(pos_q))*arg;
+                    dvel_x_turb = dvel_x_turb + std::exp((std::complex<double>)-1i*k.Dot(pos_q_dx))*arg;
+                    dvel_y_turb = dvel_y_turb + std::exp((std::complex<double>)-1i*k.Dot(pos_q_dy))*arg;
+                    dvel_z_turb = dvel_z_turb + std::exp((std::complex<double>)-1i*k.Dot(pos_q_dz))*arg;
+                }
+            }
+        }
+
+        // ToDo: check again own derivations...
+        // ToDo: modify turbulence according to local turbulent kinetic energy trb_kin_nrg_...
+        double sr2 = sqrt(2);
+        vel_turb *= sr2;
+        dvel_x_turb *= sr2;
+        dvel_y_turb *= sr2;
+        dvel_z_turb *= sr2;
+
+        // scale turbulence in proximity of ground (z=0)
+        double red_hrz_z = sqrt(ignition::math::clamp(pos.Z()/10.0,0.0,1.0));
+        double red_vrt_z = sqrt(ignition::math::clamp(pos.Z()/20.0,0.0,1.0));
+
+        // Custom mean velocity field
+        if (wind_is_valid_) {
+
+            // Calculate the x, y index of the grid points with x, y-coordinate
+            // just smaller than or equal to aircraft x, y position.
+            std::size_t x_inf = floor((pos.X() - wf_offset.X() - min_x_) / res_x_);
+            std::size_t y_inf = floor((pos.Y() - wf_offset.Y() - min_y_) / res_y_);
+            std::size_t z_inf = floor((pos.Z() - wf_offset.Z() - min_z_) / res_z_);
+
+            // Calculate the x, y index of the grid points with x, y-coordinate just
+            // greater than the aircraft x, y position.
+            std::size_t x_sup = x_inf + 1u;
+            std::size_t y_sup = y_inf + 1u;
+            std::size_t z_sup = z_inf + 1u;
+
+            // Save in an array the x, y index of each of the eight grid points
+            // enclosing the aircraft.
+            constexpr unsigned int n_vertices = 8;
+            std::size_t idx_x[n_vertices] = {x_inf,x_inf,x_inf,x_inf,x_sup,x_sup,x_sup,x_sup}; // {x_inf, x_inf, x_sup, x_sup, x_inf, x_inf, x_sup, x_sup};
+            std::size_t idx_y[n_vertices] = {y_inf,y_inf,y_sup,y_sup,y_inf,y_inf,y_sup,y_sup}; // {y_inf, y_inf, y_inf, y_inf, y_sup, y_sup, y_sup, y_sup};
+            std::size_t idx_z[n_vertices] = {z_inf,z_sup,z_inf,z_sup,z_inf,z_sup,z_inf,z_sup}; // {z_inf, z_sup, z_sup, z_inf, z_inf, z_sup, z_sup, z_inf};
+
+            // Check if aircraft is out of wind field or not, and act accordingly.
+            if (x_inf >= 0u && y_inf >= 0u && z_inf >= 0u &&
+                    x_sup <= (n_x_ - 2u) && y_sup <= (n_y_ - 2u) && z_sup <= (n_z_ - 2u)) {
+
+                // Extract the wind velocities corresponding to each vertex.
+                V3D wind_at_vertices[n_vertices];
+                V3D dwfdx_at_vertices[n_vertices];
+                V3D dwfdy_at_vertices[n_vertices];
+                V3D dwfdz_at_vertices[n_vertices];
+
+                for (std::size_t i = 0u; i < n_vertices; ++i) {
+                    wind_at_vertices[i].X() = wf_[idx_x[i] + idx_y[i] * n_x_ + idx_z[i] * n_x_ * n_y_].X();
+                    wind_at_vertices[i].Y() = wf_[idx_x[i] + idx_y[i] * n_x_ + idx_z[i] * n_x_ * n_y_].Y();
+                    wind_at_vertices[i].Z() = wf_[idx_x[i] + idx_y[i] * n_x_ + idx_z[i] * n_x_ * n_y_].Z();
+
+                    dwfdx_at_vertices[i] = dwfdx_[idx_x[i] + idx_y[i] * n_x_ + idx_z[i] * n_x_ * n_y_];
+                    dwfdy_at_vertices[i] = dwfdy_[idx_x[i] + idx_y[i] * n_x_ + idx_z[i] * n_x_ * n_y_];
+                    dwfdz_at_vertices[i] = dwfdz_[idx_x[i] + idx_y[i] * n_x_ + idx_z[i] * n_x_ * n_y_];
+                }
+
+                // Extract the relevant coordinate of every point needed for trilinear
+                double interpolation_points[14] = { min_z_+idx_z[0]*res_z_,
+                                                    min_z_+idx_z[1]*res_z_,
+                                                    min_z_+idx_z[2]*res_z_,
+                                                    min_z_+idx_z[3]*res_z_,
+                                                    min_z_+idx_z[4]*res_z_,
+                                                    min_z_+idx_z[5]*res_z_,
+                                                    min_z_+idx_z[6]*res_z_,
+                                                    min_z_+idx_z[7]*res_z_,
+                                                    min_y_+idx_y[0]*res_y_,
+                                                    min_y_+idx_y[2]*res_y_,
+                                                    min_y_+idx_y[4]*res_y_,
+                                                    min_y_+idx_y[6]*res_y_,
+                                                    min_x_+idx_x[0]*res_x_,
+                                                    min_x_+idx_x[4]*res_x_ };
+
+                // Interpolate wind velocity and and gradients at aircraft position.
+
+                vel_cust_field  = TrilinearInterpolation(pos-wf_offset, wind_at_vertices, interpolation_points);
+                dvdx_cust_field = TrilinearInterpolation(pos-wf_offset, dwfdx_at_vertices, interpolation_points);
+                dvdy_cust_field = TrilinearInterpolation(pos-wf_offset, dwfdy_at_vertices, interpolation_points);
+                //dvdz_cust_field = TrilinearInterpolation(pos-wf_offset, dwfdz_at_vertices, interpolation_points);
+            }
+        }
+
+        // Superpose everything
+        wind_queries_[iiq].wind_msg.mutable_pos_ned()->set_x(std::real(pos.X()));
+        wind_queries_[iiq].wind_msg.mutable_pos_ned()->set_y(std::real(pos.Y()));
+        wind_queries_[iiq].wind_msg.mutable_pos_ned()->set_z(std::real(pos.Z()));
+
+        wind_queries_[iiq].wind_msg.mutable_wind_ned()->set_x(std::real(vel_turb.X())*red_hrz_z+vel_lyr.X()+vel_cust_field.X());
+        wind_queries_[iiq].wind_msg.mutable_wind_ned()->set_y(std::real(vel_turb.Y())*red_hrz_z+vel_lyr.Y()+vel_cust_field.Y());
+        wind_queries_[iiq].wind_msg.mutable_wind_ned()->set_z(std::real(vel_turb.Z())*red_vrt_z+vel_lyr.Z()+vel_cust_field.Z());
+
+        wind_queries_[iiq].wind_msg.mutable_wind_grad_ned()->set_xx(std::real(dvel_x_turb.X()-vel_turb.X())*red_hrz_z+dvdx_cust_field.X());
+        wind_queries_[iiq].wind_msg.mutable_wind_grad_ned()->set_xy(std::real(dvel_y_turb.X()-vel_turb.X())*red_hrz_z+dvdy_cust_field.X());
+        wind_queries_[iiq].wind_msg.mutable_wind_grad_ned()->set_xz(std::real(dvel_z_turb.X()-vel_turb.X())*red_hrz_z+dvdz_cust_field.X());
+        wind_queries_[iiq].wind_msg.mutable_wind_grad_ned()->set_yx(std::real(dvel_x_turb.Y()-vel_turb.Y())*red_hrz_z+dvdx_cust_field.Y());
+        wind_queries_[iiq].wind_msg.mutable_wind_grad_ned()->set_yy(std::real(dvel_y_turb.Y()-vel_turb.Y())*red_hrz_z+dvdy_cust_field.Y());
+        wind_queries_[iiq].wind_msg.mutable_wind_grad_ned()->set_yz(std::real(dvel_z_turb.Y()-vel_turb.Y())*red_hrz_z+dvdz_cust_field.Y());
+        wind_queries_[iiq].wind_msg.mutable_wind_grad_ned()->set_zx(std::real(dvel_x_turb.Z()-vel_turb.Z())*red_vrt_z+dvdx_cust_field.Z());
+        wind_queries_[iiq].wind_msg.mutable_wind_grad_ned()->set_zy(std::real(dvel_y_turb.Z()-vel_turb.Z())*red_vrt_z+dvdy_cust_field.Z());
+        wind_queries_[iiq].wind_msg.mutable_wind_grad_ned()->set_zz(std::real(dvel_z_turb.Z()-vel_turb.Z())*red_vrt_z+dvdz_cust_field.Z());
+
+        wind_queries_[iiq].wind_pub->Publish(wind_queries_[iiq].wind_msg);
+
+        if (counter%100==0) {
+            gzdbg<<"turb: "<<std::real(vel_turb.X())<<" "<<std::real(vel_turb.Y())<<" "<<std::real(vel_turb.Z())<<"\n";
+            gzdbg<<"mean: "<<vel_lyr.X()<<" "<<vel_lyr.Y()<<" "<<vel_lyr.Z()<<"\n";
+            gzdbg<<"alt: "<<pos.Z()<<"\n";
+            counter = 0;
+        }
+    }
+
+    counter++;
+
+}
+
+void GazeboWindBetaPlugin::CreatePubsAndSubs() {
+    // Create temporary "ConnectGazeboToRosTopic" publisher and message.
+    gazebo::transport::PublisherPtr connect_gazebo_to_ros_topic_pub =
+            node_handle_->Advertise<gz_std_msgs::ConnectGazeboToRosTopic>(
+                "~/" + kConnectGazeboToRosSubtopic, 1);
+
+    gz_std_msgs::ConnectGazeboToRosTopic connect_gazebo_to_ros_topic_msg;
+
+    // ============================================ //
+    // ========= WRENCH STAMPED MSG SETUP ========= //
+    // ============================================ //
+    /*
+  wind_force_pub_ = node_handle_->Advertise<gz_geometry_msgs::WrenchStamped>(
+      "~/" + namespace_ + "/" + wind_force_pub_topic_, 1);
+
+  // connect_gazebo_to_ros_topic_msg.set_gazebo_namespace(namespace_);
+  connect_gazebo_to_ros_topic_msg.set_gazebo_topic("~/" + namespace_ + "/" +
+                                                   wind_force_pub_topic_);
+  connect_gazebo_to_ros_topic_msg.set_ros_topic(namespace_ + "/" +
+                                                wind_force_pub_topic_);
+  connect_gazebo_to_ros_topic_msg.set_msgtype(
+      gz_std_msgs::ConnectGazeboToRosTopic::WRENCH_STAMPED);
+  connect_gazebo_to_ros_topic_pub->Publish(connect_gazebo_to_ros_topic_msg,
+                                           true);
+  */
+    // ============================================ //
+    // ========== WIND SPEED MSG SETUP ============ //
+    // ============================================ //
+
+    for (int i=0; i<n_query_; i++) {
+        wind_queries_[i].wind_pub = node_handle_->Advertise<gz_mav_msgs::WindSpeedBeta>(
+                    "~/" + namespace_ + "/" + wind_queries_[i].wind_topic, 1);
+
+        connect_gazebo_to_ros_topic_msg.set_gazebo_topic("~/" + namespace_ + "/" +
+                                                         wind_queries_[i].wind_topic);
+        connect_gazebo_to_ros_topic_msg.set_ros_topic(namespace_ + "/" +
+                                                      wind_queries_[i].wind_topic);
+        // connect_gazebo_to_ros_topic_msg.set_msgtype(
+        //     gz_std_msgs::ConnectGazeboToRosTopic::WIND_SPEED);
+        // connect_gazebo_to_ros_topic_pub->Publish(connect_gazebo_to_ros_topic_msg,
+        //                                          true);
+    }
+}
+
+void  GazeboWindBetaPlugin::KarmanEnergySpectrum(double &E, V3D k, double L, double sigma) {
+
+    // Evaluates Karman energy spectrum at spatial frequency k
+    //
+    // Ref: J.Mann, "The spatial structure of neutral atmospheric surface-layer turbulence",
+    //      J. Fluid. Mech. (1994), vol. 273, p.146-p.147
+    //
+    // 1.4528 = 55/9*Gamma(5/6)/Gamma(1/3)/sqrt(pi)
+
+    double LK2 = L*L*k.Length()*k.Length();
+    E=1.4528*L*sigma*sigma*LK2*LK2/pow(1+LK2,17.0/6.0);
+
+}
+
+void  GazeboWindBetaPlugin::SpectralTensorIsoInc(M3D &Phi, V3D k, double L, double sigma) {
+
+    // Returns spectral tensor Phi (Fourier transform of covariance tensor) at spatial frequency k,
+    // using the Karman spectral energy.
+    //
+    // Ref: J.Mann, "The spatial structure of neutral atmospheric surface-layer turbulence",
+    //      J. Fluid. Mech. (1994), vol. 273, p.146-p.147
+
+    double E;
+    GazeboWindBetaPlugin::KarmanEnergySpectrum(E, k, L, sigma);
+
+    Phi.Set( k[1]*k[1]+k[2]*k[2], -k[0]*k[1], -k[0]*k[2],
+            -k[1]*k[0], k[0]*k[0]+k[2]*k[2], -k[1]*k[2],
+            -k[2]*k[0], -k[2]*k[1], k[0]*k[0]+k[1]*k[1]);
+
+    Phi = E/(4*M_PI*pow(k.Length(),4))*Phi;
+}
+
+void  GazeboWindBetaPlugin::SpectralTensorIsoIncDec(M3C &A, V3D k, double L, double sigma) {
+
+    // Returns decomposition A of spectral tensor Phi (Fourier transform of covariance tensor)
+    // at spatial frequency k, using the Karman spectral energy. A'A=Phi
+    //
+    // Ref: J.Mann, "Wind field simulation", Prop. Engng. Mech. (1998), vol. 13, p.280
+
+    double E;
+    GazeboWindBetaPlugin::KarmanEnergySpectrum(E, k, L, sigma);
+
+    A.Set(0, k[2], -k[1],
+            -k[2], 0, k[0],
+            k[1], -k[0], 0);
+
+    A = sqrt(E/(4*M_PI))/(k.Length()*k.Length())*A;
+}
+
+/*
+void GazeboWindBetaPlugin::LonPsdKarman(double * omega, double * phi, double n, double l, double s){
+
+     double A = 2*s*s*l/M_PI;
+     for (int i = 0; i < n; i++) {
+          double B = 1.339*l*omega[i];
+          phi[i] = A * 1/(pow(1+B*B,5/6));
+     }
+}
+
+void GazeboWindBetaPlugin::LatVertPsdKarman(double * omega, double * phi, double n, double l, double s){
+
+     double A = s*s*l/M_PI;
+     for (int i = 0; i < n; i++) {
+          double B = 1.339*l*omega[i];
+          phi[i] = A * (1+8/3*B*B)/(pow(1+B*B,11/6));
+     }
+}
+*/
+
+void GazeboWindBetaPlugin::ProcessCustomWindField() {
+
+    if (wind_is_valid_) {
+
+        for (int iix=0; iix<n_x_-1; iix++) {
+            for (int iiy=0; iiy<n_y_-1; iiy++) {
+                for (int iiz=0; iiz<n_z_-1; iiz++) {
+
+                    int i0  = iix+iiy*n_x_+iiz*n_x_*n_y_;
+                    int idx = (iix+1)+iiy*n_x_+iiz*n_x_*n_y_;
+                    int idy = iix+(iiy+1)*n_x_+iiz*n_x_*n_y_;
+                    int idz = iix+iiy*n_x_+(iiz+1)*n_x_*n_y_;
+
+                    dwfdx_.push_back((wf_[idx]-wf_[i0])/res_x_);
+                    dwfdy_.push_back((wf_[idy]-wf_[i0])/res_y_);
+                    dwfdz_.push_back((wf_[idz]-wf_[i0])/res_z_);
+                }
+            }
+        }
+    }
+}
+
+void GazeboWindBetaPlugin::ReadCustomWindFieldCSV(std::string& custom_wind_field_path) {
+
+    std::ifstream fin;
+    fin.open(custom_wind_field_path);
+
+    if (fin.is_open()) {
+
+        std::vector<float> x_coord;
+        std::vector<float> y_coord;
+        std::vector<float> z_coord;
+        int n_grid = 0;
+        double data[11];
+        char comma;
+        bool stop = false;
+        bool start = true;
+        bool got_n_x = false;
+        bool got_n_y = false;
+        bool got_n_z = false;
+
+        // Ignore header
+        fin.ignore(128, '\n');
+
+        for (;;) {
+
+            // read 11 numbers in current line and break on error
+            for (int iid=0; iid<10; iid++) {
+                fin >> data[iid] >> comma;
+                if (fin.fail() || fin.eof())
+                    stop = true;
+            }
+
+            fin >> data[10];
+
+            if (fin.fail() || fin.eof())
+                stop = true;
+
+            if (stop)
+                break;
+
+            ++n_grid;
+
+            wf_.push_back(V3D(data[1],data[2],data[3]));
+            trb_kin_nrg_.push_back(data[5]);
+
+            if (start) {
+                min_x_ = data[8];
+                min_y_ = data[9];
+                min_z_ = data[10];
+                n_x_ = 1;
+                n_y_ = 1;
+                n_z_ = 1;
+                start = false;
+
+            } else {
+                if (!got_n_x && data[8]!=min_x_) {
+                    ++n_x_;
+                    x_coord.push_back(data[8]);
+
+                } else if (data[8]==min_x_) {
+                    got_n_x=true;
+                    if (!got_n_y && data[9]!=min_y_) {
+                        ++n_y_;
+                        y_coord.push_back(data[9]);
+
+                    } else if (data[9]==min_y_) {
+                        got_n_y=true;
+                        if (!got_n_z && data[10]!=min_z_) {
+                            ++n_z_;
+                            z_coord.push_back(data[10]);
+
+                        } else {
+                            got_n_z=true;
+                        }
+                    }
+                }
+            }
+
+            // ignore rest of line
+            fin.ignore(128, '\n');
+        }
+
+        gzerr<<"Custom wind field: nx="<<n_x_<<" | ny="<<n_y_<<" | nz="<<n_z_<<"\n";
+        gzerr<<"Custom wind field: min_x="<<min_x_<<" | min_y="<<min_y_<<" | min_z="<<min_z_<<"\n";
+
+        if (n_x_>1 && n_y_>1 && n_z_>1) {
+            wind_is_valid_ = true;
+            res_x_ = x_coord[1]-x_coord[0];
+            res_y_ = y_coord[1]-y_coord[0];
+            res_z_ = z_coord[1]-z_coord[0];
+
+        } else {
+            gzerr<<"Custom wind field not valid, too few grid points (min 2x2x2).\n";
+        }
+
+    } else {
+        gzerr<<"Could not open custom wind field *.csv file. Check file permissions.\n";
+    }
+
+}
+
+/*
+void GazeboWindBetaPlugin::ReadCustomWindField(std::string& custom_wind_field_path) {
+    std::ifstream fin;
+    fin.open(custom_wind_field_path);
+    if (fin.is_open()) {
+        std::string data_name;
+        float data;
+        // Read the line with the variable name.
+        while (fin >> data_name) {
+            // Save data on following line into the correct variable.
+            if (data_name == "min_x:") {
+                fin >> min_x_;
+            } else if (data_name == "min_y:") {
+                fin >> min_y_;
+            } else if (data_name == "n_x:") {
+                fin >> n_x_;
+            } else if (data_name == "n_y:") {
+                fin >> n_y_;
+            } else if (data_name == "res_x:") {
+                fin >> res_x_;
+            } else if (data_name == "res_y:") {
+                fin >> res_y_;
+            } else if (data_name == "vertical_spacing_factors:") {
+                while (fin >> data) {
+                    vertical_spacing_factors_.push_back(data);
+                    if (fin.peek() == '\n') break;
+                }
+            } else if (data_name == "bottom_z:") {
+                while (fin >> data) {
+                    bottom_z_.push_back(data);
+                    if (fin.peek() == '\n') break;
+                }
+            } else if (data_name == "top_z:") {
+                while (fin >> data) {
+                    top_z_.push_back(data);
+                    if (fin.peek() == '\n') break;
+                }
+            } else if (data_name == "u:") {
+                while (fin >> data) {
+                    u_.push_back(data);
+                    if (fin.peek() == '\n') break;
+                }
+            } else if (data_name == "v:") {
+                while (fin >> data) {
+                    v_.push_back(data);
+                    if (fin.peek() == '\n') break;
+                }
+            } else if (data_name == "w:") {
+                while (fin >> data) {
+                    w_.push_back(data);
+                    if (fin.peek() == '\n') break;
+                }
+            } else {
+                // If invalid data name, read the rest of the invalid line,
+                // publish a message and ignore data on next line. Then resume reading.
+                std::string restOfLine;
+                getline(fin, restOfLine);
+                gzerr << " [gazebo_wind_plugin] Invalid data name '" << data_name << restOfLine <<
+                         "' in custom wind field text file. Ignoring data on next line.\n";
+                fin.ignore(std::numeric_limits<std::streamsize>::max(), '\n');
+            }
+        }
+        fin.close();
+
+        gzdbg << "[gazebo_wind_plugin] Successfully read custom wind field from text file.\n";
+
+
+
+    } else {
+        gzerr << "[gazebo_wind_plugin] Could not open custom wind field text file.\n";
+    }
+
+
+
+}
+*/
+
+ignition::math::Vector3d GazeboWindBetaPlugin::LinearInterpolation(
+        double position, ignition::math::Vector3d * values, double* points) const {
+    ignition::math::Vector3d value = values[0] + (values[1] - values[0]) /
+            (points[1] - points[0]) * (position - points[0]);
+    return value;
+}
+
+ignition::math::Vector3d GazeboWindBetaPlugin::BilinearInterpolation(
+        double* position, ignition::math::Vector3d * values, double* points) const {
+    ignition::math::Vector3d intermediate_values[2] = { LinearInterpolation(
+                                                        position[1], &(values[0]), &(points[0])),
+                                                        LinearInterpolation(
+                                                        position[1], &(values[2]), &(points[2])) };
+    ignition::math::Vector3d value = LinearInterpolation(
+                position[0], intermediate_values, &(points[4]));
+    return value;
+}
+
+ignition::math::Vector3d GazeboWindBetaPlugin::TrilinearInterpolation(
+        ignition::math::Vector3d link_position, ignition::math::Vector3d * values, double* points) const {
+    double position[3] = {link_position.X(),link_position.Y(),link_position.Z()};
+
+    /*
+    gzerr<<"Trilipol:   Pos: "<<position[0]<<","<<position[1]<<","<<position[2]<<"\n";
+    std::stringstream ss;
+    ss<<"         Points: ";
+    for(int i=0; i<14; i++){
+        ss<<points[i]<<", ";
+    }
+    gzerr<<ss.str()<<"\n";
+    gzerr<<"           Wind: "<<values[0].X()<<", "<<values[1].X()<<", "<<values[2].X()<<", "<<values[3].X()<<", "<<values[4].X()<<", "<<values[5].X()<<", "<<values[6].X()<<", "<<values[7].X()<<"\n";
+    gzerr<<"                 "<<values[0].Y()<<", "<<values[1].Y()<<", "<<values[2].Y()<<", "<<values[3].Y()<<", "<<values[4].Y()<<", "<<values[5].Y()<<", "<<values[6].Y()<<", "<<values[7].Y()<<"\n";
+    gzerr<<"                 "<<values[0].Z()<<", "<<values[1].Z()<<", "<<values[2].Z()<<", "<<values[3].Z()<<", "<<values[4].Z()<<", "<<values[5].Z()<<", "<<values[6].Z()<<", "<<values[7].Z()<<"\n";
+
+    gzerr<<"                 nx="<<n_x_<<" | ny="<<n_y_<<" | nz="<<n_z_<<"\n";
+    gzerr<<"                 min_x="<<min_x_<<" | min_y="<<min_y_<<" | min_z="<<min_z_<<"\n";
+    gzerr<<"                 res_x="<<res_x_<<" | res_y="<<res_y_<<" | res_z="<<res_z_<<"\n";
+    */
+
+    ignition::math::Vector3d intermediate_values[4] = { LinearInterpolation(
+                                                        position[2], &(values[0]), &(points[0])),
+                                                        LinearInterpolation(
+                                                        position[2], &(values[2]), &(points[2])),
+                                                        LinearInterpolation(
+                                                        position[2], &(values[4]), &(points[4])),
+                                                        LinearInterpolation(
+                                                        position[2], &(values[6]), &(points[6])) };
+    ignition::math::Vector3d value = BilinearInterpolation(
+                &(position[0]), intermediate_values, &(points[8]));
+    /*
+    gzerr<<"                 vx="<<value.X()<<" | vy="<<value.Y()<<" | vz="<<value.Z()<<"\n";
+    */
+
+    return value;
+}
+
+GZ_REGISTER_MODEL_PLUGIN(GazeboWindBetaPlugin);
+
+}  // namespace gazebo
diff --git a/rotors_gazebo_plugins/include/rotors_gazebo_plugins/gazebo_wind_beta_plugin.h b/rotors_gazebo_plugins/include/rotors_gazebo_plugins/gazebo_wind_beta_plugin.h
new file mode 100644
index 000000000..68c8aebd3
--- /dev/null
+++ b/rotors_gazebo_plugins/include/rotors_gazebo_plugins/gazebo_wind_beta_plugin.h
@@ -0,0 +1,303 @@
+/*
+ * Copyright 2015 Fadri Furrer, ASL, ETH Zurich, Switzerland
+ * Copyright 2015 Michael Burri, ASL, ETH Zurich, Switzerland
+ * Copyright 2015 Mina Kamel, ASL, ETH Zurich, Switzerland
+ * Copyright 2015 Janosch Nikolic, ASL, ETH Zurich, Switzerland
+ * Copyright 2015 Markus Achtelik, ASL, ETH Zurich, Switzerland
+ * Copyright 2016 Geoffrey Hunter <gbmhunter@gmail.com>
+ *
+ * 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 ROTORS_GAZEBO_PLUGINS_GAZEBO_WIND_BETA_PLUGIN_H
+#define ROTORS_GAZEBO_PLUGINS_GAZEBO_WIND_BETA_PLUGIN_H
+
+#include <string>
+#include <random>
+#include <complex>
+#include <cmath>
+#include <algorithm>
+#include <iostream>
+#include <fstream>
+
+#include <gazebo/common/common.hh>
+#include <gazebo/common/Plugin.hh>
+#include <gazebo/gazebo.hh>
+#include <gazebo/physics/physics.hh>
+#include <eigen3/Eigen/Dense>
+
+#include <mav_msgs/default_topics.h>  // This comes from the mav_comm repo
+
+#include "rotors_gazebo_plugins/common.h"
+
+#include "WindSpeed.pb.h"             // Wind speed message
+#include "WindSpeedBeta.pb.h"         // Wind speed message
+#include "WrenchStamped.pb.h"         // Wind force message
+
+namespace gazebo {
+
+typedef ignition::math::Vector3d V3D;
+typedef ignition::math::Vector3<std::complex<double>> V3C;
+typedef ignition::math::Matrix3d M3D;
+typedef ignition::math::Matrix3<std::complex<double>> M3C;
+
+/// \brief    This gazebo plugin simulates wind acting on a model.
+/// \details  This plugin publishes on a Gazebo topic and instructs the ROS interface plugin to
+///           forward the message onto ROS.
+class GazeboWindBetaPlugin : public ModelPlugin {
+ public:
+  GazeboWindBetaPlugin()
+      : ModelPlugin(),
+        namespace_(kDefaultNamespace),
+        node_handle_(nullptr),
+        pubs_and_subs_created_(false) {}
+
+  virtual ~GazeboWindBetaPlugin();
+
+ protected:
+
+  /// \brief Load the plugin.
+  /// \param[in] _model Pointer to the model that loaded this plugin.
+  /// \param[in] _sdf SDF element that describes the plugin.
+  void Load(physics::ModelPtr _model, sdf::ElementPtr _sdf);
+
+  /// \brief Called when the world is updated.
+  /// \param[in] _info Update timing information.
+  void OnUpdate(const common::UpdateInfo& /*_info*/);
+
+ private:
+
+  /// \brief    Flag that is set to true once CreatePubsAndSubs() is called, used
+  ///           to prevent CreatePubsAndSubs() from be called on every OnUpdate().
+  bool pubs_and_subs_created_;
+
+  /// \brief    Creates all required publishers and subscribers, incl. routing of messages to/from ROS if required.
+  /// \details  Call this once the first time OnUpdate() is called (can't
+  ///           be called from Load() because there is no guarantee GazeboRosInterfacePlugin has
+  ///           has loaded and listening to ConnectGazeboToRosTopic and ConnectRosToGazeboTopic messages).
+  void CreatePubsAndSubs();
+
+  /// \brief    Pointer to the update event connection.
+  event::ConnectionPtr update_connection_;
+
+  physics::WorldPtr world_;
+  physics::ModelPtr model_;
+  physics::LinkPtr link_;
+
+  std::string namespace_;
+
+  struct WindQuery {
+     physics::LinkPtr link;
+     std::string frame_id;
+     std::string wind_topic;
+     gazebo::transport::PublisherPtr wind_pub;
+     gz_mav_msgs::WindSpeedBeta wind_msg;
+
+     ignition::math::Vector3d wind_ned;
+     ignition::math::Vector3d pos_ned;
+     ignition::math::Matrix3d wind_grad_ned;
+     bool sdf_valid = true;
+  };
+
+  WindQuery * wind_queries_;
+  int n_query_ = 0;
+
+  struct WindLayer {
+     double altitude = 0.0;   // lower altitude of wind layer, [m]
+     //bool rel_alt = false;    // altitude over ground (true) or sea-level (false)
+     //double blend_alt = 0.0;  // blending interval to smoothly mix between different layers [m]
+
+     V3D wind_mean = V3D(0,0,0);             // spatiotemporal mean wind vector in NED [m/s]
+
+     /*
+     V3D trb_sigma_nrm = V3D(0.1,0.1,0.1);   // sigma/|wind_mean| of lon/lat/vert turbulence components [-]
+     V3D trb_length = V3D(762,762,762);      // turbulence length scale of lon/lat/vert components [m]
+
+     int n_omega = 100;   // number of spatial frequencies to be used
+     double * omega;      // array of wavenumbers: omega_i = 2*pi/lambda_i [rad/m]
+     double d_omega;
+     double * phi_u;      // longitudinal turbulence psd,  [m³/s²]
+     double * phi_v;      // lateral turbulence psd,  [m³/s²]
+     double * phi_w;      // vertical turbulence psd,  [m³/s²]
+
+     int n_theta = 21;    // number of planar spreading directions in [-pi/2,pi/2[
+     double * theta;      // array of planar wave-directions, e[-pi/2,pi/2[ [rad]
+     double d_theta;
+
+     double gust_speed_mean = 0.0;
+     double gust_speed_variance = 0.0;
+     V3D gust_ned_direction = V3D(1,0,0);
+
+     common::Time wind_gust_period_mean = 0.0;
+     common::Time wind_gust_period_variance = 0.0;
+     common::Time wind_gust_duration_mean = 0.0;
+     common::Time wind_gust_duration_variance = 0.0;
+
+     void InitializeLayer(){
+        double omega_low =  2*M_PI/1e3;   // wavelength <= 1000m
+        double omega_high = 2*M_PI/0.5;   // wavelength >= 0.5m (!aliasing! check sim-freq vs max. air-speed)
+
+        omega = new double [n_omega];
+        phi_u = new double [n_omega];
+        phi_v = new double [n_omega];
+        phi_w = new double [n_omega];
+
+        d_omega = (omega_high-omega_low)/(n_omega-1);
+
+        for (int i = 0; i < n_omega; i++) {
+            omega[i] = i*d_omega;
+        }
+
+        //GazeboWindBetaPlugin::LonPsdKarman(omega, phi_u, n_omega, trb_length[0], trb_sigma_nrm[0]*wind_mean.Length());
+        //GazeboWindBetaPlugin::LatVertPsdKarman(omega, phi_v, n_omega, trb_length[1], trb_sigma_nrm[1]*wind_mean.Length());
+        //GazeboWindBetaPlugin::LatVertPsdKarman(omega, phi_w, n_omega, trb_length[2], trb_sigma_nrm[2]*wind_mean.Length());
+
+        theta = new double [n_theta];
+        d_theta = M_PI/(n_theta);
+
+        for (int i = 0; i < n_theta; i++) {
+            theta[i] = i*d_theta;
+        }
+     }
+     */
+  };
+
+  WindLayer * wind_layers_;
+  int * layer_index_alt_incr_;
+  int n_layer_ = 0;
+
+  common::Time wind_gust_end_;
+  common::Time wind_gust_start_;
+
+  // Turbulence: Fourier Simulation
+  int counter=0;
+
+  double lambda_min_ = 2.5;
+  double L_ = 725;
+  double sigma_ = 1.5;
+
+  V3D trnsp_vel_turb_ = V3D(0,0,0);
+
+  int nk_x_ = 15;
+  int nk_y_ = 15;
+  int nk_z_ = 15;
+
+  double dk_x_;// = 2*M_PI/lambda_min_/nk_x_;
+  double dk_y_;// = 2*M_PI/lambda_min_/nk_y_;
+  double dk_z_;// = 2*M_PI/lambda_min_/nk_z_;
+
+  M3C *** C_ij_; //M3C C_ij_[nk_x_][nk_y_][nk_z_];
+  V3C *** rand_phase_; //V3C rand_phase_[nk_x_][nk_y_][nk_z_];
+
+  void KarmanEnergySpectrum(double &E, V3D k, double L, double sigma);
+
+  void SpectralTensorIsoInc(M3D &Phi, V3D k, double L, double sigma);
+
+  void SpectralTensorIsoIncDec(M3C &A, V3D k, double L, double sigma);
+
+  static void PrintMatrix(M3C M);
+
+  static bool SortByAltitude(const WindLayer &L1, const WindLayer &L2) {return L1.altitude<L2.altitude;}
+
+  /*
+  /// \brief Returns power spectral density phi(omega) [m³/s²] of the
+  /// longitudinal Karman turbulence model. n=length of passed arrays.
+  /// l = turbulence length scale [m], s = turbulence rms [m²/s²]
+  void LonPsdKarman(double * omega, double * phi, double n, double l, double s);
+
+  /// \brief Returns power spectral density phi(omega) of the
+  /// lateral/vertical Karman turbulence model. n=length of passed arrays.
+  /// l = turbulence length scale [m], s = turbulence rms [m²/s²]
+  void LatVertPsdKarman(double * omega, double * phi, double n, double l, double s);
+ */
+
+  //  Variables for custom wind field generation.
+  bool wind_is_valid_ = false;
+  double min_x_ = 0;
+  double min_y_ = 0;
+  double min_z_ = 0;
+  int n_x_ = 0;
+  int n_y_ = 0;
+  int n_z_ = 0;
+  double res_x_ = 1;
+  double res_y_ = 1;
+  double res_z_ = 1;
+  V3D wf_offset = V3D(0,0,0);
+  std::vector<V3D> wf_;
+  std::vector<V3D> dwfdx_;
+  std::vector<V3D> dwfdy_;
+  std::vector<V3D> dwfdz_;
+  std::vector<double> trb_kin_nrg_;
+
+  /// \brief  Reads wind data from a text file and saves it.
+  /// \param[in] custom_wind_field_path Path to the wind field from ~/.ros.
+  void ReadCustomWindFieldCSV(std::string& custom_wind_field_path);
+
+  /// \brief  Calculates gradients of wind data.
+  void ProcessCustomWindField();
+
+  // \brief  Reads wind data from a text file and saves it.
+  // \param[in] custom_wind_field_path Path to the wind field from ~/.ros.
+  //void ReadCustomWindField(std::string& custom_wind_field_path);
+
+  /// \brief  Functions for trilinear interpolation of wind field at aircraft position.
+  
+  /// \brief  Linear interpolation
+  /// \param[in]  position y-coordinate of the target point.
+  ///             values Pointer to an array of size 2 containing the wind values
+  ///                    of the two points to interpolate from (12 and 13).
+  ///             points Pointer to an array of size 2 containing the y-coordinate 
+  ///                    of the two points to interpolate from.
+  ignition::math::Vector3d LinearInterpolation(double position, ignition::math::Vector3d * values, double* points) const;
+  
+  /// \brief  Bilinear interpolation
+  /// \param[in]  position Pointer to an array of size 2 containing the x- and 
+  ///                      y-coordinates of the target point.
+  ///             values Pointer to an array of size 4 containing the wind values 
+  ///                    of the four points to interpolate from (8, 9, 10 and 11).
+  ///             points Pointer to an array of size 14 containing the z-coordinate
+  ///                    of the eight points to interpolate from, the x-coordinate 
+  ///                    of the four intermediate points (8, 9, 10 and 11), and the 
+  ///                    y-coordinate of the last two intermediate points (12 and 13).
+  ignition::math::Vector3d BilinearInterpolation(double* position, ignition::math::Vector3d * values, double* points) const;
+  
+  /// \brief  Trilinear interpolation
+  /// \param[in]  link_position Vector3 containing the x, y and z-coordinates
+  ///                           of the target point.
+  ///             values Pointer to an array of size 8 containing the wind values of the 
+  ///                    eight points to interpolate from (0, 1, 2, 3, 4, 5, 6 and 7).
+  ///             points Pointer to an array of size 14 containing the z-coordinate          
+  ///                    of the eight points to interpolate from, the x-coordinate 
+  ///                    of the four intermediate points (8, 9, 10 and 11), and the 
+  ///                    y-coordinate of the last two intermediate points (12 and 13).
+  ignition::math::Vector3d TrilinearInterpolation(ignition::math::Vector3d link_position, ignition::math::Vector3d * values, double* points) const;
+  
+  gazebo::transport::PublisherPtr wind_force_pub_;
+  gazebo::transport::PublisherPtr wind_speed_pub_;
+
+  gazebo::transport::NodePtr node_handle_;
+
+  /// \brief    Gazebo message for sending wind data.
+  /// \details  This is defined at the class scope so that it is re-created
+  ///           everytime a wind message needs to be sent, increasing performance.
+  gz_geometry_msgs::WrenchStamped wrench_stamped_msg_;
+
+  /// \brief    Gazebo message for sending wind speed data.
+  /// \details  This is defined at the class scope so that it is re-created
+  ///           everytime a wind speed message needs to be sent, increasing performance.
+  gz_mav_msgs::WindSpeed wind_speed_msg_;
+};
+}
+
+#endif // ROTORS_GAZEBO_PLUGINS_GAZEBO_WIND_BETA_PLUGIN_H
diff --git a/rotors_gazebo_plugins/include/rotors_gazebo_plugins/uav_parameters.h b/rotors_gazebo_plugins/include/rotors_gazebo_plugins/uav_parameters.h
new file mode 100644
index 000000000..626e623c7
--- /dev/null
+++ b/rotors_gazebo_plugins/include/rotors_gazebo_plugins/uav_parameters.h
@@ -0,0 +1,413 @@
+/*
+ * Copyright 2017 Pavel Vechersky, ASL, ETH Zurich, Switzerland
+ *
+ * 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 ROTORS_GAZEBO_PLUGINS_UAV_PARAMETERS_H_
+#define ROTORS_GAZEBO_PLUGINS_UAV_PARAMETERS_H_
+
+#include <Eigen/Dense>
+#include <yaml-cpp/yaml.h>
+#include <exception>
+
+namespace gazebo {
+
+/*
+// Forward declaration.
+struct ControlSurface;
+
+/// \brief  Wrapper function for extracting control surface parameters from a
+///         YAML node.
+inline void YAMLReadAirfoil(const YAML::Node& node,
+                                   const std::string& name,
+                                   ControlSurface& surface);
+*/
+
+//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
+// Default airfoil parameter values (NACAXXXX airfoil and low-Re flat-plate)
+//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
+// Non-stalled regime
+static constexpr double kDefaultAlphaMaxNs = 0.27;
+static constexpr double kDefaultAlphaMinNs = -0.27;
+
+static const Eigen::Vector3d kDefaultCLiftAlpha =
+    Eigen::Vector3d(0.4, 6.3, 0.0);
+
+static const Eigen::Vector3d kDefaultCDragAlpha =
+    Eigen::Vector3d(0.0, 0.0, 0.9);
+
+static const Eigen::Vector2d kDefaultCPitchMomentAlpha =
+    Eigen::Vector2d(-0.1, 0.0);
+
+// Stalled regime
+static constexpr double kDefaultAlphaBlend = 0.2;
+static constexpr double kDefaultFpCLiftMax = 0.65;
+static constexpr double kDefaultFpCDragMax = 1.20;
+static constexpr double kDefaultFpCPitchMomentMax = 0.40;
+
+//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
+// Default propeller parameter values (similar 11x7E apc)
+//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
+static constexpr double kDefaultDiameter = 0.28;
+static constexpr double kDefaultMass = 0.023;
+static constexpr double kDefaultKT = -0.13;
+static constexpr double kDefaultKT0 = 0.11;
+static constexpr double kDefaultKQ = -0.011;
+static constexpr double kDefaultKQ0 = 0.01;
+static constexpr double kDefaultRollMomCoeff = 1e-06;
+static constexpr double kDefaultDragMomCoeff = 5.3849e-04;
+static constexpr double kDefaultDFlow = 5.0;
+
+//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
+// Default values for use with ADIS16448 IMU
+//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
+static constexpr double kDefaultAdisGyroscopeNoiseDensity = 2.0 * 35.0 / 3600.0 / 180.0 * M_PI;
+static constexpr double kDefaultAdisGyroscopeRandomWalk = 2.0 * 4.0 / 3600.0 / 180.0 * M_PI;
+static constexpr double kDefaultAdisGyroscopeBiasCorrelationTime = 1.0e+3;
+static constexpr double kDefaultAdisGyroscopeTurnOnBiasSigma = 0.5 / 180.0 * M_PI;
+static constexpr double kDefaultAdisAccelerometerNoiseDensity = 2.0 * 2.0e-3;
+static constexpr double kDefaultAdisAccelerometerRandomWalk = 2.0 * 3.0e-3;
+static constexpr double kDefaultAdisAccelerometerBiasCorrelationTime = 300.0;
+static constexpr double kDefaultAdisAccelerometerTurnOnBiasSigma = 20.0e-3 * 9.8;
+
+//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
+// Earth's gravity in Zurich (lat=+47.3667degN, lon=+8.5500degE, h=+500m, WGS84)
+//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
+static constexpr double kDefaultGravityMagnitude = 9.8068;
+
+//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
+
+/// \brief  This function reads a vector from a YAML node and converts it into
+///         a vector of type Eigen.
+template <typename Derived>
+inline void YAMLReadEigenVector(const YAML::Node& node,
+                                const std::string& name,
+                                Eigen::MatrixBase<Derived>& value);
+
+/// \brief  This function reads a parameter from a YAML node.
+template <typename T>
+inline void YAMLReadParam(const YAML::Node& node,
+                          const std::string& name,
+                          T& value);
+
+/// \brief  Macros to reduce copies of names.
+//#define READ_CONTROL_SURFACE(node, item) YAMLReadControlSurface(node, #item, item);
+#define READ_EIGEN_VECTOR(node, item) YAMLReadEigenVector(node, #item, item);
+#define READ_PARAM(node, item) YAMLReadParam(node, #item, item);
+
+//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
+
+struct PropellerParameters {
+
+    PropellerParameters():
+        diameter(kDefaultDiameter),
+        mass(kDefaultMass),
+        k_t(kDefaultKT),
+        k_t0(kDefaultKT0),
+        k_q(kDefaultKQ),
+        k_q0(kDefaultKQ0),
+        rolling_moment_coefficient(kDefaultRollMomCoeff),
+        rotor_drag_coefficient(kDefaultDragMomCoeff),
+        d_flow(kDefaultDFlow){}
+
+    double diameter;
+    double mass;
+    double k_t;
+    double k_t0;
+    double k_q;
+    double k_q0;
+    double rolling_moment_coefficient;
+    double rotor_drag_coefficient;
+    double d_flow;
+
+    void LoadPropParamsYAML(const std::string& yaml_path) {
+
+        gzdbg <<"loading propeller: "<< yaml_path <<std::endl;
+
+        try{
+
+            const YAML::Node node = YAML::LoadFile(yaml_path);
+
+            try{
+
+                READ_PARAM(node, diameter);
+                READ_PARAM(node, mass);
+                READ_PARAM(node, k_t);
+                READ_PARAM(node, k_t0);
+                READ_PARAM(node, k_q);
+                READ_PARAM(node, k_q0);
+                READ_PARAM(node, rolling_moment_coefficient);
+                READ_PARAM(node, rotor_drag_coefficient);
+                READ_PARAM(node, d_flow);
+
+            } catch(const YAML::Exception& ex) {
+                gzerr << ex.what();
+            } catch (const std::exception& ex) {
+                gzerr<<ex.what()<<std::endl;
+            } catch (const std::string& ex) {
+                gzerr<<ex<<std::endl;
+            } catch (...) {
+                gzerr<<"meeep"<<std::endl;
+            }
+
+        } catch(const YAML::Exception& ex) {
+            gzerr << ex.what();
+        } catch (const std::exception& ex) {
+            gzerr<<ex.what()<<std::endl;
+        } catch (const std::string& ex) {
+            gzerr<<ex<<std::endl;
+        } catch (...) {
+            gzerr<<"meeep"<<std::endl;
+        }
+    }
+};
+
+struct AerodynamicParameters {
+    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+    AerodynamicParameters():
+          alpha_max_ns(kDefaultAlphaMaxNs),
+          alpha_min_ns(kDefaultAlphaMinNs),
+          c_lift_alpha(kDefaultCLiftAlpha),
+          c_drag_alpha(kDefaultCDragAlpha),
+          c_pitch_moment_alpha(kDefaultCPitchMomentAlpha),
+          alpha_blend(kDefaultAlphaBlend),
+          fp_c_lift_max(kDefaultFpCLiftMax),
+          fp_c_drag_max(kDefaultFpCDragMax),
+          fp_c_pitch_moment_max(kDefaultFpCPitchMomentMax) {
+
+          //gzdbg<<"aerodynamic struct created... \n";
+    }
+
+    double alpha_max_ns;
+    double alpha_min_ns;
+
+    Eigen::Vector3d c_lift_alpha;
+    Eigen::Vector3d c_drag_alpha;
+    Eigen::Vector2d c_pitch_moment_alpha;
+
+    double alpha_blend;
+    double fp_c_lift_max;
+    double fp_c_drag_max;
+    double fp_c_pitch_moment_max;
+
+    void LoadAeroParamsYAML(const std::string& yaml_path) {
+
+        gzdbg <<"loading airfoil: "<< yaml_path <<std::endl;
+
+        try{
+
+            const YAML::Node node = YAML::LoadFile(yaml_path);
+
+            try{
+
+                READ_PARAM(node, alpha_max_ns);
+                READ_PARAM(node, alpha_min_ns);
+
+                READ_EIGEN_VECTOR(node, c_lift_alpha);
+                READ_EIGEN_VECTOR(node, c_drag_alpha);
+                READ_EIGEN_VECTOR(node, c_pitch_moment_alpha);
+
+                READ_PARAM(node, alpha_blend);
+                READ_PARAM(node, fp_c_lift_max);
+                READ_PARAM(node, fp_c_drag_max);
+                READ_PARAM(node, fp_c_pitch_moment_max);
+
+            } catch(const YAML::Exception& ex) {
+                gzerr << ex.what();
+            } catch (const std::exception& ex) {
+                gzerr<<ex.what()<<std::endl;
+            } catch (const std::string& ex) {
+                gzerr<<ex<<std::endl;
+            } catch (...) {
+                gzerr<<"meeep"<<std::endl;
+            }
+
+        } catch(const YAML::Exception& ex) {
+            gzerr << ex.what();
+        } catch (const std::exception& ex) {
+            gzerr<<ex.what()<<std::endl;
+        } catch (const std::string& ex) {
+            gzerr<<ex<<std::endl;
+        } catch (...) {
+            gzerr<<"meeep"<<std::endl;
+        }
+    }
+};
+
+/*
+struct ControlSurface {
+  ControlSurface(int cs_channel,
+                 double defl_min = kDefaultControlSurfaceDeflectionMin,
+                 double defl_max = kDefaultControlSurfaceDeflectionMax)
+      : channel(cs_channel),
+        deflection_min(defl_min),
+        deflection_max(defl_max) {}
+
+  int channel;
+
+  double deflection_min;
+  double deflection_max;
+
+  void LoadControlSurfaceNode(const YAML::Node& node) {
+    READ_PARAM(node, channel);
+    READ_PARAM(node, deflection_min);
+    READ_PARAM(node, deflection_max);
+  }
+};
+*/
+/*
+struct FWVehicleParameters {
+  FWVehicleParameters()
+      : wing_span(kDefaultWingSpan),
+        wing_surface(kDefaultWingSurface),
+        chord_length(kDefaultChordLength),
+        thrust_inclination(kDefaultThrustInclination),
+        throttle_channel(kDefaultThrottleChannel),
+        aileron_left(kDefaultAileronLeftChannel),
+        aileron_right(kDefaultAileronRightChannel),
+        elevator(kDefaultElevatorChannel),
+        flap(kDefaultFlapChannel),
+        rudder(kDefaultRudderChannel) {}
+
+  double wing_span;
+  double wing_surface;
+  double chord_length;
+  double thrust_inclination;
+
+  int throttle_channel;
+
+  ControlSurface aileron_left;
+  ControlSurface aileron_right;
+  ControlSurface elevator;
+  ControlSurface flap;
+  ControlSurface rudder;
+
+  void LoadVehicleParamsYAML(const std::string& yaml_path) {
+    const YAML::Node node = YAML::LoadFile(yaml_path);
+
+    try{
+    READ_PARAM(node, wing_span);
+    READ_PARAM(node, wing_surface);
+    READ_PARAM(node, chord_length);
+    READ_PARAM(node, thrust_inclination);
+
+    READ_PARAM(node, throttle_channel);
+
+    READ_CONTROL_SURFACE(node, aileron_left);
+    READ_CONTROL_SURFACE(node, aileron_right);
+    READ_CONTROL_SURFACE(node, elevator);
+    READ_CONTROL_SURFACE(node, flap);
+    READ_CONTROL_SURFACE(node, rudder);
+
+    } catch (const std::exception& ex) {
+        gzerr<<ex.what()<<std::endl;
+    } catch (const std::string& ex) {
+        gzerr<<ex<<std::endl;
+    } catch (...) {
+        gzerr<<"meeep"<<std::endl;
+    }
+  }
+};
+*/
+/*
+inline void YAMLReadControlSurface(const YAML::Node& node,
+                                   const std::string& name,
+                                   ControlSurface& surface) {
+  const YAML::Node surface_node = node[name];
+  surface.LoadControlSurfaceNode(surface_node);
+}
+*/
+
+// A description of the parameters:
+// https://github.com/ethz-asl/kalibr/wiki/IMU-Noise-Model-and-Intrinsics
+// TODO(burrimi): Should I have a minimalistic description of the params here?
+struct ImuParameters {
+  /// Gyroscope noise density (two-sided spectrum) [rad/s/sqrt(Hz)]
+  double gyroscope_noise_density;
+  /// Gyroscope bias random walk [rad/s/s/sqrt(Hz)]
+  double gyroscope_random_walk;
+  /// Gyroscope bias correlation time constant [s]
+  double gyroscope_bias_correlation_time;
+  /// Gyroscope turn on bias standard deviation [rad/s]
+  double gyroscope_turn_on_bias_sigma;
+  /// Accelerometer noise density (two-sided spectrum) [m/s^2/sqrt(Hz)]
+  double accelerometer_noise_density;
+  /// Accelerometer bias random walk. [m/s^2/s/sqrt(Hz)]
+  double accelerometer_random_walk;
+  /// Accelerometer bias correlation time constant [s]
+  double accelerometer_bias_correlation_time;
+  /// Accelerometer turn on bias standard deviation [m/s^2]
+  double accelerometer_turn_on_bias_sigma;
+  /// Norm of the gravitational acceleration [m/s^2]
+  double gravity_magnitude;
+
+  ImuParameters()
+      : gyroscope_noise_density(kDefaultAdisGyroscopeNoiseDensity),
+        gyroscope_random_walk(kDefaultAdisGyroscopeRandomWalk),
+        gyroscope_bias_correlation_time(
+            kDefaultAdisGyroscopeBiasCorrelationTime),
+        gyroscope_turn_on_bias_sigma(kDefaultAdisGyroscopeTurnOnBiasSigma),
+        accelerometer_noise_density(kDefaultAdisAccelerometerNoiseDensity),
+        accelerometer_random_walk(kDefaultAdisAccelerometerRandomWalk),
+        accelerometer_bias_correlation_time(
+            kDefaultAdisAccelerometerBiasCorrelationTime),
+        accelerometer_turn_on_bias_sigma(
+            kDefaultAdisAccelerometerTurnOnBiasSigma),
+        gravity_magnitude(kDefaultGravityMagnitude) {}
+};
+
+template <typename Derived>
+inline void YAMLReadEigenVector(const YAML::Node& node,
+                                const std::string& name,
+                                Eigen::MatrixBase<Derived>& value) {
+    std::vector<typename Derived::RealScalar> vec =
+            node[name].as<std::vector<typename Derived::RealScalar>>();
+    assert(vec.size() == Derived::SizeAtCompileTime);
+    value = Eigen::Map<Derived>(&vec[0], vec.size());
+}
+
+template <typename T>
+inline void YAMLReadParam(const YAML::Node& node,
+                          const std::string& name,
+                          T& value) {
+    value = node[name].as<T>();
+}
+
+}
+
+template<typename ValueType>
+bool SafeGet(const YAML::Node& node, const std::string& key, ValueType* value) {
+  //CHECK_NOTNULL(value);
+  bool success = false;
+  if(!node.IsMap()) {
+    gzerr << "Unable to get Node[\"" << key << "\"] because the node is not a map";
+  } else {
+    const YAML::Node sub_node = node[key];
+    if(sub_node) {
+      try {
+        *value = sub_node.as<ValueType>();
+        success = true;
+      } catch(const YAML::Exception& e) {
+        gzerr << "Error getting key \"" << key << "\" as type "
+            << typeid(ValueType).name() << ": " << e.what();
+      }
+    } else {
+      gzerr << "Key \"" << key << "\" does not exist";
+    }
+  }
+  return success;
+}
+
+#endif /* ROTORS_GAZEBO_PLUGINS_UAV_PARAMETERS_H_ */
diff --git a/rotors_gazebo_plugins/msgs/ArrowMarker.proto b/rotors_gazebo_plugins/msgs/ArrowMarker.proto
new file mode 100644
index 000000000..a7193ddbd
--- /dev/null
+++ b/rotors_gazebo_plugins/msgs/ArrowMarker.proto
@@ -0,0 +1,20 @@
+syntax = "proto2";
+package gz_visualization_msgs;
+
+import "vector3d.proto";
+//import "Header.proto";
+
+// VisVector is used to plot any vector in rviz, e.g. lift and drag for a wing segment
+message ArrowMarker
+{
+
+  //required gz_std_msgs.Header header = 1; // stamp & link frame in/to which the below data is relative to/expressed
+
+  required int32 id = 2; // unique marker id
+  required string ns = 3;
+  required gazebo.msgs.Vector3d startpoint = 4; // Vector starting point
+  required gazebo.msgs.Vector3d vector = 5;	// Vector
+  required gazebo.msgs.Vector3d scale = 6;	// Vector shaft diam./head diam./head length
+  required gazebo.msgs.Vector3d color = 7;	// RGB values [0,1]
+
+}
diff --git a/rotors_gazebo_plugins/msgs/ConnectGazeboToRosTopic.proto b/rotors_gazebo_plugins/msgs/ConnectGazeboToRosTopic.proto
index d4dbbfd52..07c9418ba 100644
--- a/rotors_gazebo_plugins/msgs/ConnectGazeboToRosTopic.proto
+++ b/rotors_gazebo_plugins/msgs/ConnectGazeboToRosTopic.proto
@@ -30,6 +30,8 @@ message ConnectGazeboToRosTopic
     VECTOR_3D_STAMPED = 12;
     WIND_SPEED = 13;
     WRENCH_STAMPED = 14;
+    VIS_VECTOR = 15;
+    VIS_VECTOR_ARRAY = 16;
   }
   required MsgType msgType = 4;
 }
diff --git a/rotors_gazebo_plugins/msgs/PropulsionSlipstream.proto b/rotors_gazebo_plugins/msgs/PropulsionSlipstream.proto
new file mode 100644
index 000000000..8f126fb2d
--- /dev/null
+++ b/rotors_gazebo_plugins/msgs/PropulsionSlipstream.proto
@@ -0,0 +1,19 @@
+syntax = "proto2";
+package gz_mav_msgs;
+import "vector3d.proto";
+import "time.proto";
+
+message PropulsionSlipstream
+{
+    required gazebo.msgs.Vector3d rotor_pos = 1;
+    required gazebo.msgs.Vector3d ind_vel_disk = 2;
+    required gazebo.msgs.Vector3d ind_vel_end = 3;
+    required gazebo.msgs.Vector3d wake_dir = 4;
+
+    required double l_a = 5;
+    required double l_p = 6;
+
+    optional double k_w = 7;
+    optional double prop_diam = 8;
+    optional gazebo.msgs.Time timestamp = 9;
+}
diff --git a/rotors_gazebo_plugins/msgs/VisVector.proto b/rotors_gazebo_plugins/msgs/VisVector.proto
new file mode 100644
index 000000000..4480b1477
--- /dev/null
+++ b/rotors_gazebo_plugins/msgs/VisVector.proto
@@ -0,0 +1,20 @@
+syntax = "proto2";
+package gz_visualization_msgs;
+
+import "vector3d.proto";
+import "Header.proto";
+
+// VisVector is used to plot any vector in rviz, e.g. lift and drag for a wing segment
+message VisVector
+{
+
+  required gz_std_msgs.Header header = 1; // stamp & link frame in/to which the below data is relative to/expressed
+
+  required int32 id = 2; // unique marker id
+  required string ns = 3;
+  required gazebo.msgs.Vector3d startpoint = 4; // Vector starting point
+  required gazebo.msgs.Vector3d vector = 5;	// Vector
+  required gazebo.msgs.Vector3d scale = 6;	// Vector shaft diam./head diam./head length
+  required gazebo.msgs.Vector3d color = 7;	// RGB values [0,1]
+
+}
diff --git a/rotors_gazebo_plugins/msgs/VisVectorArray.proto b/rotors_gazebo_plugins/msgs/VisVectorArray.proto
new file mode 100644
index 000000000..0f9987600
--- /dev/null
+++ b/rotors_gazebo_plugins/msgs/VisVectorArray.proto
@@ -0,0 +1,14 @@
+syntax = "proto2";
+package gz_visualization_msgs;
+
+import "ArrowMarker.proto";
+import "Header.proto";
+
+// VisVectorArray is used to plot multiple vectors in rviz, e.g. lift and drag of wing segments
+message VisVectorArray
+{
+
+  required gz_std_msgs.Header header = 1; 		// stamp & link frame in/to which the below data is relative to/expressed
+  repeated gz_visualization_msgs.ArrowMarker vector = 2;	// arrow-type marker
+
+}
diff --git a/rotors_gazebo_plugins/msgs/WindSpeedBeta.proto b/rotors_gazebo_plugins/msgs/WindSpeedBeta.proto
new file mode 100644
index 000000000..44535b98d
--- /dev/null
+++ b/rotors_gazebo_plugins/msgs/WindSpeedBeta.proto
@@ -0,0 +1,13 @@
+syntax = "proto2";
+package gz_mav_msgs;
+
+import "Header.proto";
+import "vector3d.proto";
+import "matrix3d.proto";
+
+message WindSpeedBeta {
+
+  required gazebo.msgs.Vector3d pos_ned = 2;
+  required gazebo.msgs.Vector3d wind_ned = 3;
+  required gz_std_msgs.Matrix3d wind_grad_ned = 4;
+}
diff --git a/rotors_gazebo_plugins/msgs/matrix3d.proto b/rotors_gazebo_plugins/msgs/matrix3d.proto
new file mode 100644
index 000000000..a3093ebaa
--- /dev/null
+++ b/rotors_gazebo_plugins/msgs/matrix3d.proto
@@ -0,0 +1,15 @@
+syntax = "proto2";
+package gz_std_msgs;
+
+message Matrix3d
+{
+  required double xx = 1;
+  required double yx = 2;
+  required double zx = 3;
+  required double xy = 4;
+  required double yy = 5;
+  required double zy = 6;
+  required double xz = 7;
+  required double yz = 8;
+  required double zz = 9;
+}
diff --git a/rotors_gazebo_plugins/src/gazebo_aerodynamics_plugin.cpp b/rotors_gazebo_plugins/src/gazebo_aerodynamics_plugin.cpp
new file mode 100644
index 000000000..2e15f7cbc
--- /dev/null
+++ b/rotors_gazebo_plugins/src/gazebo_aerodynamics_plugin.cpp
@@ -0,0 +1,822 @@
+/*
+ * Copyright (C) 2014-2016 Open Source Robotics Foundation
+ *
+ * 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.
+ *
+ *
+ * Modifications by David Rohr, drohr@student.ethz.ch
+ *
+ */
+
+#include "rotors_gazebo_plugins/gazebo_aerodynamics_plugin.h"
+
+using namespace gazebo;
+
+GZ_REGISTER_MODEL_PLUGIN(GazeboAerodynamics)
+
+/////////////////////////////////////////////////
+GazeboAerodynamics::~GazeboAerodynamics()
+{
+
+    for(int i=0; i<n_seg_; i++){
+        delete[] segments_[i].slpstr;
+        delete[] segments_[i].cs;
+        delete[] segments_[i].wind;
+    }
+    delete[] segments_;
+    delete[] bodies_;
+
+    gzdbg<<"liftdrag destructed"<<std::endl;
+}
+
+/////////////////////////////////////////////////
+void GazeboAerodynamics::Load(physics::ModelPtr _model,
+                              sdf::ElementPtr _sdf)
+{
+    gzdbg<<"Load started\n";
+
+    GZ_ASSERT(_model, "GazeboAerodynamics _model pointer is NULL");
+    GZ_ASSERT(_sdf, "GazeboAerodynamics _sdf pointer is NULL");
+    this->model = _model;
+
+    this->world = this->model->GetWorld();
+    GZ_ASSERT(this->world, "GazeboAerodynamics world pointer is NULL");
+
+    GZ_ASSERT(_sdf, "GazeboAerodynamics _sdf pointer is NULL");
+
+    namespace_.clear();
+
+    /*
+    gzdbg<<"model name: "<<model->GetName()<<"\n";
+    model->Print("");
+    gzdbg<<"model child count: "<<model->GetChildCount()<<"\n";
+    */
+
+    if (_sdf->HasElement("robotNamespace"))
+        namespace_ = _sdf->GetElement("robotNamespace")->Get<std::string>();
+    else
+        gzerr << "[gazebo_motor_model] Please specify a robotNamespace.\n";
+
+    node_handle_ = transport::NodePtr(new transport::Node());
+    node_handle_->Init();
+
+    if (_sdf->HasElement("linkName")) {
+        sdf::ElementPtr elem = _sdf->GetElement("linkName");
+        // GZ_ASSERT(elem, "Element link_name doesn't exist!");
+        std::string linkName = elem->Get<std::string>();
+        this->link = this->model->GetLink(linkName);
+        // GZ_ASSERT(this->link, "Link was NULL");
+
+        if (!this->link) {
+            gzerr << "Link with name[" << linkName << "] not found.\n";
+        }
+    }
+
+    last_time_ = world->SimTime(); // ini last time
+
+    if (_sdf->HasElement("aeroForcesVis")) {
+        vector_vis_array_topic_ = _sdf->Get<std::string>("aeroForcesVis");
+    } else {
+        vector_vis_array_topic_ = "aero_forces_vis";
+    }
+
+    if (_sdf->HasElement("body")) {
+
+        sdf::ElementPtr _sdf_body = _sdf->GetElement("body");
+        sdf::ElementPtr _sdf_element = _sdf_body->GetElement("element");
+
+        while (_sdf_element) {
+            _sdf_element = _sdf_element->GetNextElement("element");
+            ++n_bdy_;
+        }
+
+        gzdbg<<"found "<<n_bdy_<<" body element(s) for this link. \n";
+        bodies_ = new Body [n_bdy_];
+
+        _sdf_element = _sdf_body->GetElement("element");
+
+        for(int i=0; i<n_bdy_; i++){
+            gzdbg<<"processing body-element nr: "<<i<<" \n";
+
+            if (_sdf_element->HasElement("linkNameRef")) {
+                bodies_[i].ref_link =  this->model->GetLink(_sdf_element->Get<std::string>("linkNameRef"));
+
+                if (bodies_[i].ref_link == NULL)
+                    gzthrow("Couldn't find specified reference link: "<<_sdf_element->Get<std::string>("linkNameRef")<<"\n");
+
+            } else if (this->link) {
+                bodies_[i].ref_link = this->link;     // for backward-compatibility with older xacro airframes... to be updated
+                gzerr<<"Using linkName for linkNameRef.\n";
+
+            } else {
+                gzthrow("No reference link specified\n");
+            }
+
+            if (_sdf_element->HasElement("linkNameAct")) {
+                bodies_[i].act_link =  model->GetLink(_sdf_element->Get<std::string>("linkNameAct"));
+
+                if (bodies_[i].act_link == NULL)
+                    gzthrow("Couldn't find specified link to act on: "<<_sdf_element->Get<std::string>("linkNameAct")<<"\n");
+
+            } else {
+                bodies_[i].act_link = bodies_[i].ref_link;
+                gzerr<<"Using linkNameRef for linkNameAct.\n";
+            }
+
+            if (_sdf_element->HasElement("forward"))
+                bodies_[i].fwd = _sdf_element->Get<V3D>("forward");
+            else
+                gzwarn<<"segment ["<<i<<"] is missing 'forward' element \n";
+
+            bodies_[i].fwd.Normalize();
+
+            if (_sdf_element->HasElement("upward"))
+                bodies_[i].upwd = _sdf_element->Get<V3D>("upward");
+            else
+                gzwarn<<"segment ["<<i<<"] is missing 'upward' elemenbodiest \n";
+
+            bodies_[i].upwd.Normalize();
+
+            if (_sdf_element->HasElement("cp"))
+                bodies_[i].cp = _sdf_element->Get<V3D>("cp");
+            else
+                gzwarn<<"segment ["<<i<<"] is missing 'cp' element \n";
+
+            if (_sdf_element->HasElement("aBdyXX"))
+                bodies_[i].a_fus_xx = _sdf_element->Get<double>("aBdyXX");
+            else
+                gzwarn<<"segment ["<<i<<"] is missing 'aBdyXX' element \n";
+
+            if (_sdf_element->HasElement("aBdyYY"))
+                bodies_[i].a_fus_yy = _sdf_element->Get<double>("aBdyYY");
+            else
+                gzwarn<<"segment ["<<i<<"] is missing 'aBdyYY' element \n";
+
+            if (_sdf_element->HasElement("aBdyZZ"))
+                bodies_[i].a_fus_zz = _sdf_element->Get<double>("aBdyZZ");
+            else
+                gzwarn<<"segment ["<<i<<"] is missing 'aBdyZZ' element \n";
+
+            // get winds
+            if (_sdf_element->HasElement("wind")) {
+                sdf::ElementPtr _sdf_wind= _sdf_element->GetElement("wind");
+
+                // get number of winds for this particular segment
+                while (_sdf_wind) {
+                    _sdf_wind = _sdf_wind->GetNextElement("wind");
+                    ++bodies_[i].n_wind;
+                }
+
+                gzdbg<<"found "<<bodies_[i].n_wind<<" wind(s) for body ["<<i<<"]. \n";
+                _sdf_wind = _sdf_element->GetElement("wind");
+                bodies_[i].wind = new Wind [bodies_[i].n_wind];
+
+                for(int j=0; j<bodies_[i].n_wind; j++){
+
+                    if(_sdf_wind->HasElement("topic")){
+                        bodies_[i].wind[j].wind_topic = _sdf_wind->Get<std::string>("topic");
+                    } else {
+                        gzwarn<<"wind ["<<j<<"] of body ["<<i<<"] is missing 'topic' element \n";
+                    }
+
+                    _sdf_wind = _sdf_wind->GetNextElement("wind");
+                }
+            }
+
+            if (_sdf_element->HasElement("VecVisTopic")){
+                bodies_[i].vector_vis_array_topic = _sdf_element->Get<std::string>("VecVisTopic");
+                if(bodies_[i].vector_vis_array_topic.empty())
+                    gzdbg<<"No visualization topic specified, won't publish.\n";
+            } else {
+                std::stringstream ss;
+                ss << vector_vis_array_topic_ + "/body_" + std::to_string(i);
+                bodies_[i].vector_vis_array_topic = ss.str();
+            }
+
+            bodies_[i].vector_vis_array_msg.mutable_header()->mutable_stamp()->set_sec(0.0);
+            bodies_[i].vector_vis_array_msg.mutable_header()->mutable_stamp()->set_nsec(0.0);
+            bodies_[i].vector_vis_array_msg.mutable_header()->set_frame_id(bodies_[i].ref_link->GetName());
+
+            for (int j = 0; j < BODY_N_VIS_VEC; j++) {
+                bodies_[i].vec_vis[j] = bodies_[i].vector_vis_array_msg.add_vector();
+                bodies_[i].vec_vis[j]->set_id(1);
+                bodies_[i].vec_vis[j]->mutable_scale()->set_x(0.025);
+                bodies_[i].vec_vis[j]->mutable_scale()->set_y(0.05);
+                bodies_[i].vec_vis[j]->mutable_scale()->set_z(0.05);
+
+                switch (j) {
+                  case 0: bodies_[i].vec_vis[j]->set_ns(namespace_+"/bdy/force"); break;
+                  case 1: bodies_[i].vec_vis[j]->set_ns(namespace_+"/bdy/wind"); break;
+                  default: break;
+                }
+            }
+        }
+    }
+
+    if (_sdf->HasElement("airfoil")) {
+
+        sdf::ElementPtr _sdf_airfoil = _sdf->GetElement("airfoil");
+        sdf::ElementPtr _sdf_segment = _sdf_airfoil->GetElement("segment");
+
+        while (_sdf_segment) {
+            _sdf_segment = _sdf_segment->GetNextElement("segment");
+            ++n_seg_;
+        }
+
+        gzdbg<<"found "<<n_seg_<<" airfoil segment(s) for this link. \n";
+        segments_ = new Segment [n_seg_];
+
+        _sdf_segment = _sdf_airfoil->GetElement("segment");
+
+        for(int i=0; i<n_seg_; i++){
+            gzdbg<<"processing airfoil-segment nr: "<<i<<" \n";
+
+            if (_sdf_segment->HasElement("linkNameRef")) {
+                segments_[i].ref_link =  this->model->GetLink(_sdf_segment->Get<std::string>("linkNameRef"));
+
+                if (segments_[i].ref_link == NULL)
+                    gzthrow("Couldn't find specified reference link: "<<_sdf_segment->Get<std::string>("linkNameRef")<<"\n");
+
+            } else if (this->link) {
+                segments_[i].ref_link = this->link;     // for backward-compatibility with older xacro airframes... to be updated
+                gzerr<<"Using linkName for linkNameRef.\n";
+
+            } else {
+                gzthrow("No reference link specified\n");
+            }
+
+            if (_sdf_segment->HasElement("linkNameAct")) {
+                segments_[i].act_link =  model->GetLink(_sdf_segment->Get<std::string>("linkNameAct"));
+
+                if (segments_[i].act_link == NULL)
+                    gzthrow("Couldn't find specified link to act on: "<<_sdf_segment->Get<std::string>("linkNameAct")<<"\n");
+
+            } else {
+                segments_[i].act_link = segments_[i].ref_link;
+                gzerr<<"Using linkNameRef for linkNameAct.\n";
+            }
+
+            if (_sdf_segment->HasElement("forward"))
+                segments_[i].fwd = _sdf_segment->Get<V3D>("forward");
+            else
+                gzwarn<<"segment ["<<i<<"] is missing 'forward' element \n";
+
+            segments_[i].fwd.Normalize();
+
+            if (_sdf_segment->HasElement("upward"))
+                segments_[i].upwd = _sdf_segment->Get<V3D>("upward");
+            else
+                gzwarn<<"segment ["<<i<<"] is missing 'upward' element \n";
+
+            segments_[i].upwd.Normalize();
+
+            if (_sdf_segment->HasElement("cp"))
+                segments_[i].cp = _sdf_segment->Get<V3D>("cp");
+            else
+                gzwarn<<"segment ["<<i<<"] is missing 'cp' element \n";
+
+            if (_sdf_segment->HasElement("seg_area"))
+                segments_[i].seg_area = _sdf_segment->Get<double>("seg_area");
+            else
+                gzwarn<<"segment ["<<i<<"] is missing 'seg_area' element \n";
+
+            if (_sdf_segment->HasElement("seg_chord"))
+                segments_[i].seg_chord = _sdf_segment->Get<double>("seg_chord");
+            else
+                gzwarn<<"segment ["<<i<<"] is missing 'seg_chord' element \n";
+
+            if (_sdf_segment->HasElement("aeroParamsYAML")) {
+                std::string aero_params_yaml =
+                        _sdf_segment->GetElement("aeroParamsYAML")->Get<std::string>();
+                segments_[i].aero_params.LoadAeroParamsYAML(aero_params_yaml);
+
+            } else {
+                gzwarn<<"segment ["<<i<<"] is missing aerodynamic paramaters YAML file, "
+                     <<"using default parameters.\n";
+            }
+
+            // get control joints
+            if (_sdf_segment->HasElement("control")) {
+                sdf::ElementPtr _sdf_control = _sdf_segment->GetElement("control");
+                sdf::ElementPtr _sdf_cs = _sdf_control->GetElement("cs");
+
+                // get number of control inputs on this particular segment
+                while (_sdf_cs) {
+                    _sdf_cs = _sdf_cs->GetNextElement("cs");
+                    ++segments_[i].n_cs;
+                }
+
+                gzdbg<<"found "<<segments_[i].n_cs<<" control surface segment(s) for segment ["<<i<<"]. \n";
+                _sdf_cs = _sdf_control->GetElement("cs");
+                segments_[i].cs = new ControlSurface [segments_[i].n_cs];
+
+                for(int j=0; j<segments_[i].n_cs; j++){
+
+                    if(_sdf_cs->HasElement("fromTopic"))
+                        segments_[i].cs[j].from_topic = _sdf_cs->Get<bool>("fromTopic");
+
+                    if (_sdf_cs->HasElement("controlJoint")){
+                        std::string joint_name = _sdf_cs->Get<std::string>("controlJoint");
+                        segments_[i].cs[j].control_joint = model->GetJoint(joint_name);
+
+                        if (model->GetJoint(joint_name) == nullptr)
+                            gzwarn << "joint [" << joint_name << "] not found \n";
+
+                    } else if(!segments_[i].cs[j].from_topic) {
+                        gzwarn<<"control surface ["<<j<<"] of segment ["<<i<<"] is missing 'controlJoint' element and won't be effective \n";
+                    }
+
+                    if(segments_[i].cs[j].from_topic){
+                        if (_sdf_cs->HasElement("csRefTopic")) {
+                            segments_[i].cs[j].cs_ref_topic = _sdf_cs->Get<std::string>("csRefTopic");
+                        } else {
+                            gzwarn<<"control surface ["<<j<<"] of segment ["<<i<<"] is missing 'csRefTopic' element and won't be effective \n";
+                            segments_[i].cs[j].from_topic = false;
+                        }
+                    }
+
+                    if (_sdf_cs->HasElement("radToCLift")) {
+                        segments_[i].cs[j].control_joint_rad_to_cl = _sdf_cs->Get<V3D>("radToCLift");
+                    } else {
+                        gzwarn<<"control surface ["<<j<<"] of segment ["<<i<<"] is missing 'radToCLift' element \n";
+                    }
+
+                    if (_sdf_cs->HasElement("radToCDrag"))
+                        segments_[i].cs[j].control_joint_rad_to_cd = _sdf_cs->Get<V3D>("radToCDrag");
+                    else
+                        gzwarn<<"control surface ["<<j<<"] of segment ["<<i<<"] is missing 'radToCDrag' element \n";
+
+                    if (_sdf_cs->HasElement("radToCPitch"))
+                        segments_[i].cs[j].control_joint_rad_to_cm = _sdf_cs->Get<V3D>("radToCPitch");
+                    else
+                        gzwarn<<"control surface ["<<j<<"] of segment ["<<i<<"] is missing 'radToCPitch' element \n";
+
+                    if (_sdf_cs->HasElement("radToAoAB"))
+                        segments_[i].cs[j].d_aoa_b_d_delta_cs = _sdf_cs->Get<double>("radToAoAB");
+                    else
+                        gzwarn<<"control surface ["<<j<<"] of segment ["<<i<<"] is missing 'radToAoAB' element \n";
+
+                    _sdf_cs = _sdf_cs->GetNextElement("cs");
+                }
+            }
+
+            // get slipstreams
+            if (_sdf_segment->HasElement("indVel")) {
+                sdf::ElementPtr _sdf_ind_vel= _sdf_segment->GetElement("indVel");
+                //sdf::ElementPtr _sdf_slpstr = _sdf_ind_vel->GetElement("slpstr");
+
+                // get number of slipstreams for this particular segment
+                while (_sdf_ind_vel) {
+                    _sdf_ind_vel = _sdf_ind_vel->GetNextElement("indVel");
+                    ++segments_[i].n_slpstr;
+                }
+
+                gzdbg<<"found "<<segments_[i].n_slpstr<<" slipstream(s) for segment ["<<i<<"]. \n";
+                _sdf_ind_vel = _sdf_segment->GetElement("indVel");
+                segments_[i].slpstr = new Slipstream [segments_[i].n_slpstr];
+
+                for(int j=0; j<segments_[i].n_slpstr; j++){
+
+                    if(_sdf_ind_vel->HasElement("topic")){
+                        segments_[i].slpstr[j].slpstr_topic = _sdf_ind_vel->Get<std::string>("topic");
+                    } else {
+                        gzwarn<<"slipstream ["<<j<<"] of segment ["<<i<<"] is missing 'topic' element \n";
+                    }
+
+                    _sdf_ind_vel = _sdf_ind_vel->GetNextElement("indVel");
+                }
+            }
+
+            // get winds
+            if (_sdf_segment->HasElement("wind")) {
+                sdf::ElementPtr _sdf_wind= _sdf_segment->GetElement("wind");
+
+                // get number of winds for this particular segment
+                while (_sdf_wind) {
+                    _sdf_wind = _sdf_wind->GetNextElement("wind");
+                    ++segments_[i].n_wind;
+                }
+
+                gzdbg<<"found "<<segments_[i].n_wind<<" wind(s) for segment ["<<i<<"]. \n";
+                _sdf_wind = _sdf_segment->GetElement("wind");
+                segments_[i].wind = new Wind [segments_[i].n_wind];
+
+                for(int j=0; j<segments_[i].n_wind; j++){
+
+                    if(_sdf_wind->HasElement("topic")){
+                        segments_[i].wind[j].wind_topic = _sdf_wind->Get<std::string>("topic");
+                    } else {
+                        gzwarn<<"wind ["<<j<<"] of segment ["<<i<<"] is missing 'topic' element \n";
+                    }
+
+                    _sdf_wind = _sdf_wind->GetNextElement("wind");
+                }
+            }
+
+            if (_sdf_segment->HasElement("VecVisTopic")) {
+                segments_[i].vector_vis_array_topic = _sdf_segment->Get<std::string>("VecVisTopic");
+                if(segments_[i].vector_vis_array_topic.empty())
+                    gzdbg<<"No visualization topic specified, won't publish.\n";
+            } else {
+                std::stringstream ss;
+                ss << vector_vis_array_topic_ + "/segment_" + std::to_string(i);
+                segments_[i].vector_vis_array_topic = ss.str();;
+            }
+
+            segments_[i].vector_vis_array_msg.mutable_header()->mutable_stamp()->set_sec(0.0);
+            segments_[i].vector_vis_array_msg.mutable_header()->mutable_stamp()->set_nsec(0.0);
+            segments_[i].vector_vis_array_msg.mutable_header()->set_frame_id(segments_[i].ref_link->GetName());
+
+            for (int j = 0; j < AIRFOIL_N_VIS_VEC; j++) {
+                segments_[i].vec_vis[j] = segments_[i].vector_vis_array_msg.add_vector();
+                segments_[i].vec_vis[j]->set_id(1);
+                segments_[i].vec_vis[j]->mutable_scale()->set_x(0.025);
+                segments_[i].vec_vis[j]->mutable_scale()->set_y(0.05);
+                segments_[i].vec_vis[j]->mutable_scale()->set_z(0.05);
+
+                switch (j) {
+                  case 0: segments_[i].vec_vis[j]->set_ns(namespace_+"/F_L"); break;
+                  case 1: segments_[i].vec_vis[j]->set_ns(namespace_+"/F_D"); break;
+                  case 2: segments_[i].vec_vis[j]->set_ns(namespace_+"/M_M"); break;
+                  case 3: segments_[i].vec_vis[j]->set_ns(namespace_+"/F_tot"); break;
+                  case 4: segments_[i].vec_vis[j]->set_ns(namespace_+"/wind"); break;
+                  case 5: segments_[i].vec_vis[j]->set_ns(namespace_+"/slipstream"); break;
+                  default: break;
+                }
+            }
+
+            _sdf_segment = _sdf_segment->GetNextElement("segment");
+        }
+    }
+
+    this->updateConnection = event::Events::ConnectWorldUpdateBegin(boost::bind(&GazeboAerodynamics::OnUpdate, this));
+    gzdbg<<"Load completed, ConnectWorldUpdateBegin called\n";
+
+}
+
+/////////////////////////////////////////////////
+
+void GazeboAerodynamics::OnUpdate()
+{
+    if (!pubs_and_subs_created_) {
+
+        gazebo::transport::PublisherPtr connect_gazebo_to_ros_topic_pub =
+                node_handle_->Advertise<gz_std_msgs::ConnectGazeboToRosTopic>(
+                    "~/" + kConnectGazeboToRosSubtopic, 1);
+        gz_std_msgs::ConnectGazeboToRosTopic connect_gazebo_to_ros_topic_msg;
+        gzdbg<<"advertised ~/" + kConnectGazeboToRosSubtopic + "\n";
+
+        for (int i = 0; i < n_seg_; i++) {
+            for (int j = 0; j < segments_[i].n_slpstr; j++) {
+                segments_[i].slpstr[j].propulsion_slipstream_sub_ = node_handle_->Subscribe("~/" + namespace_ + "/" + segments_[i].slpstr[j].slpstr_topic,
+                                                                                            &GazeboAerodynamics::Slipstream::Callback,
+                                                                                            &segments_[i].slpstr[j]);
+                gzdbg<<"subscribing to: "<<"~/" + namespace_ + "/" + segments_[i].slpstr[j].slpstr_topic<<"\n";
+            }
+
+            for (int j = 0; j < segments_[i].n_wind; j++) {
+                segments_[i].wind[j].wind_sub_ = node_handle_->Subscribe("~/" + namespace_ + "/" + segments_[i].wind[j].wind_topic,
+                                                                                            &GazeboAerodynamics::Wind::Callback,
+                                                                                            &segments_[i].wind[j]);
+                gzdbg<<"subscribing to: "<<"~/" + namespace_ + "/" + segments_[i].wind[j].wind_topic<<"\n";
+            }
+
+            for (int j = 0; j < segments_[i].n_cs; j++) {
+                if(segments_[i].cs[j].from_topic){
+                    segments_[i].cs[j].control_ref_sub = node_handle_->Subscribe("~/" + namespace_ + "/" + segments_[i].cs[j].cs_ref_topic,
+                                                                                 &GazeboAerodynamics::ControlSurface::Callback,
+                                                                                 &segments_[i].cs[j]);
+                    gzdbg<<"sub to: ~/" + namespace_ + "/" + segments_[i].cs[j].cs_ref_topic + "\n";
+                }
+            }
+
+            if (!segments_[i].vector_vis_array_topic.empty()) {
+                segments_[i].vector_vis_array_pub = node_handle_->Advertise<gz_visualization_msgs::VisVectorArray>
+                        ("~/" + namespace_ + "/" + segments_[i].vector_vis_array_topic, 1);
+                connect_gazebo_to_ros_topic_msg.set_gazebo_topic("~/" + namespace_ + "/" + segments_[i].vector_vis_array_topic);
+                connect_gazebo_to_ros_topic_msg.set_ros_topic(namespace_ + "/" + segments_[i].vector_vis_array_topic);
+                connect_gazebo_to_ros_topic_msg.set_msgtype(gz_std_msgs::ConnectGazeboToRosTopic::VIS_VECTOR_ARRAY);
+                connect_gazebo_to_ros_topic_pub->Publish(connect_gazebo_to_ros_topic_msg, true);
+            }
+        }
+
+        for (int i = 0; i < n_bdy_; i++) {
+            for (int j = 0; j < bodies_[i].n_wind; j++) {
+                bodies_[i].wind[j].wind_sub_ = node_handle_->Subscribe("~/" + namespace_ + "/" + bodies_[i].wind[j].wind_topic,
+                                                                         &GazeboAerodynamics::Wind::Callback,
+                                                                         &bodies_[i].wind[j]);
+                gzdbg<<"subscribing to: "<<"~/" + namespace_ + "/" + bodies_[i].wind[j].wind_topic<<"\n";
+            }
+
+            if (!bodies_[i].vector_vis_array_topic.empty()) {
+                bodies_[i].vector_vis_array_pub = node_handle_->Advertise<gz_visualization_msgs::VisVectorArray>
+                        ("~/" + namespace_ + "/" + bodies_[i].vector_vis_array_topic, 1);
+                connect_gazebo_to_ros_topic_msg.set_gazebo_topic("~/" + namespace_ + "/" + bodies_[i].vector_vis_array_topic);
+                connect_gazebo_to_ros_topic_msg.set_ros_topic(namespace_ + "/" + bodies_[i].vector_vis_array_topic);
+                connect_gazebo_to_ros_topic_msg.set_msgtype(gz_std_msgs::ConnectGazeboToRosTopic::VIS_VECTOR_ARRAY);
+                connect_gazebo_to_ros_topic_pub->Publish(connect_gazebo_to_ros_topic_msg, true);
+            }
+        }
+
+        pubs_and_subs_created_ = true;
+    }
+
+    common::Time current_time = world->SimTime();
+    double dt = (current_time - last_time_).Double();
+    last_time_ = current_time;
+
+    if (n_seg_>0) {
+        // iterate over employed wing segments
+        for (int i = 0; i<n_seg_; i++) {
+
+            // pose of reference link
+            GZ_ASSERT(segments_[i].ref_link, "Link was NULL");
+#if GAZEBO_MAJOR_VERSION >= 9
+            ignition::math::Pose3d pose = segments_[i].ref_link->WorldPose();
+#else
+            ignition::math::Pose3d pose = ignitionFromGazeboMath(segments_[i].ref_link->GetWorldPose());
+#endif
+
+            V3D cp = pose.Pos() + pose.Rot().RotateVector(segments_[i].cp);  // segments cp-reference position expressed in world-frame
+
+            segments_[i].UpdateIndVel(cp);
+            segments_[i].UpdateWind(cp);
+
+            // get local airspeed at cp, expressed in world frame
+#if GAZEBO_MAJOR_VERSION >= 9
+            V3D vel = segments_[i].ref_link->WorldLinearVel(segments_[i].cp);
+#else
+            V3D vel = ignitionFromGazeboMath(segments_[i].ref_link->GetWorldLinearVel(segments_[i].cp));
+#endif
+
+            // account for induced velocity and wind, expressed in world frame
+            vel = vel - segments_[i].v_ind_cp - segments_[i].wind_cp;
+
+            // inflow direction at cp, expressed in world frame
+            V3D velI = vel.Normalized();
+
+            // forward, upward, spanwise direction, expressed in world frame
+            V3D forward_i = pose.Rot().RotateVector(segments_[i].fwd);
+            V3D upward_i = pose.Rot().RotateVector(segments_[i].upwd);
+            V3D spanwise_i = forward_i.Cross(upward_i).Normalized();
+
+            // velocity in lift-drag plane
+            V3D vel_in_ld_plane = vel - vel.Dot(spanwise_i)*spanwise_i;
+
+            // get direction of drag
+            V3D drag_dir = -vel_in_ld_plane;
+            drag_dir.Normalize();
+
+            // get direction of lift
+            V3D lift_dir = spanwise_i.Cross(vel_in_ld_plane);
+            lift_dir.Normalize();
+
+            // get direction of moment
+            V3D moment_dir = spanwise_i;
+
+            // get sweep (angle between velI and lift-drag-plane) (aka sideslip)
+            //double cos_sweep_angle = ignition::math::clamp(velI.Dot(-drag_dir), -1.0, 1.0);
+
+            // compute angle of attack
+            double alpha = acos(-drag_dir.Dot(forward_i)); // [0,pi]
+            if (drag_dir.Dot(upward_i)<=0) {
+                alpha*=-1;
+            }
+
+            //segments_[i].alpha_dot = (alpha-segments_[i].alpha_prev)/ignition::math::clamp(dt,0.001,0.1);
+            //segments_[i].alpha_prev = alpha;
+
+            // compute dynamic pressure
+            double q = 0.5 * rho_ * vel_in_ld_plane.Dot(vel_in_ld_plane);
+
+            // aerodynamic coefficient correction terms due to control surface deflection (only effective if not stalled)
+            double d_cl = 0.0;
+            double d_cd = 0.0;
+            double d_cm = 0.0;
+
+            // shift of stall angle due to control surface deflection
+            double d_alpha_max_ns = 0.0;
+
+            for(int j=0; j<segments_[i].n_cs; j++){
+                double control_defl;
+                if (segments_[i].cs[j].from_topic) {
+                    control_defl = segments_[i].cs[j].cs_ref;
+
+                } else if (segments_[i].cs[j].control_joint){
+#if GAZEBO_MAJOR_VERSION >= 9
+                    control_defl = segments_[i].cs[j].control_joint->Position(0);
+#else
+                    control_defl = segments_[i].cs[j].control_joint->GetAngle(0).Radian();
+#endif
+                } else {
+                    control_defl = 0.0;
+                }
+
+                if (!std::isfinite(control_defl))
+                    control_defl = 0.0;
+
+                d_cl += segments_[i].cs[j].control_joint_rad_to_cl[2] +
+                        segments_[i].cs[j].control_joint_rad_to_cl[0]*control_defl +
+                        segments_[i].cs[j].control_joint_rad_to_cl[1]*control_defl*control_defl;
+
+                d_cd += segments_[i].cs[j].control_joint_rad_to_cd[2] +
+                        segments_[i].cs[j].control_joint_rad_to_cd[0]*control_defl +
+                        segments_[i].cs[j].control_joint_rad_to_cd[1]*control_defl*control_defl;
+
+                d_cm += segments_[i].cs[j].control_joint_rad_to_cm[2] +
+                        segments_[i].cs[j].control_joint_rad_to_cm[0]*control_defl +
+                        segments_[i].cs[j].control_joint_rad_to_cm[1]*control_defl*control_defl;
+
+                d_alpha_max_ns += segments_[i].cs[j].d_aoa_b_d_delta_cs*control_defl;
+            }
+
+            // assembling aerodynamic coefficients for pre-stall (af) and post-stall (fp)
+            Eigen::Vector3d alpha_poly_3(1.0,alpha,alpha*alpha);
+            Eigen::Vector2d alpha_poly_2(1.0,alpha);
+
+            double cl_af = segments_[i].aero_params.c_lift_alpha.dot(alpha_poly_3) + d_cl;
+            double cd_af = segments_[i].aero_params.c_drag_alpha.dot(alpha_poly_3) + d_cd;
+            double cm_af = segments_[i].aero_params.c_pitch_moment_alpha.dot(alpha_poly_2) + d_cm;
+
+            double cl_fp = segments_[i].aero_params.fp_c_lift_max*sin(2*alpha);
+            double cd_fp = segments_[i].aero_params.fp_c_drag_max*pow(sin(alpha),2);
+            double cm_fp = -segments_[i].aero_params.fp_c_pitch_moment_max*sin(pow(alpha,3)/(M_PI*M_PI));
+
+            // form mixing weight to combine pre- and post-stall models
+            double w_af;
+            if(alpha>segments_[i].aero_params.alpha_max_ns + segments_[i].aero_params.alpha_blend + d_alpha_max_ns)
+                w_af = 0.0;
+            else if(alpha>segments_[i].aero_params.alpha_max_ns + d_alpha_max_ns)
+                w_af = 0.5+0.5*cos(M_PI*(alpha - segments_[i].aero_params.alpha_max_ns)/segments_[i].aero_params.alpha_blend);
+            else if(alpha>segments_[i].aero_params.alpha_min_ns + d_alpha_max_ns)
+                w_af = 1.0;
+            else if(alpha>segments_[i].aero_params.alpha_min_ns - segments_[i].aero_params.alpha_blend + d_alpha_max_ns)
+                w_af = 0.5+0.5*cos(M_PI*(segments_[i].aero_params.alpha_min_ns-alpha)/segments_[i].aero_params.alpha_blend);
+            else
+                w_af = 0.0;
+
+            // form weighted sum of pre-stall and post_stall (flat-plate) contributions to aerodynamic coefficients
+            double cl = w_af*cl_af+(1-w_af)*cl_fp;
+            double cd = w_af*cd_af+(1-w_af)*cd_fp;
+            double cm = w_af*cm_af+(1-w_af)*cm_fp;
+
+            // original plugin incorporates sweep (sideslip)
+            //cl = cl*cos_sweep_angle;
+            //cd = cd*cos_sweep_angle;
+            //cm = cm*cos_sweep_angle;
+
+            // set to zero if desired...
+            //cm = 0.0;
+            //cd = 0.0;
+            //cl = 0.0;
+
+            // assemble final forces and moments
+            V3D lift   = cl * q * segments_[i].seg_area * lift_dir;
+            V3D drag   = cd * q * segments_[i].seg_area * drag_dir;
+            V3D moment = cm * q * segments_[i].seg_area * moment_dir * segments_[i].seg_chord;
+
+            V3D force  = lift + drag;
+            V3D torque = moment;
+
+            // correct for nan or inf
+            force.Correct();
+            cp.Correct();
+            torque.Correct();
+
+            // apply forces cp
+            segments_[i].act_link->AddForceAtWorldPosition(force, cp);
+            segments_[i].act_link->AddTorque(torque);
+
+            // visualization
+            if (segments_[i].vector_vis_array_pub) {
+
+                // world to body frame
+                V3D _B_wind = pose.Rot().RotateVectorReverse(segments_[i].wind_cp);
+                V3D _B_slipstream = pose.Rot().RotateVectorReverse(segments_[i].v_ind_cp);
+                V3D _B_lift = pose.Rot().RotateVectorReverse(lift);
+                V3D _B_drag = pose.Rot().RotateVectorReverse(drag);
+                V3D _B_torque = pose.Rot().RotateVectorReverse(torque);
+                V3D _B_force = pose.Rot().RotateVectorReverse(force);
+
+                float r,g,b;
+                V3D P_start = segments_[i].cp;
+                V3D P_vec;
+
+                for(int j = 0; j < segments_[i].vec_vis.size(); j++){
+
+                    switch (j) {
+                      case 0: r = 1 - w_af; g = w_af; b = 0; P_vec = _B_lift;       break;
+                      case 1: r = 1 - w_af; g = w_af; b = 0; P_vec = _B_drag;       break;
+                      case 2: r = 1 - w_af; g = w_af; b = 0; P_vec = _B_torque;     break;
+                      case 3: r = 1 - w_af; g = w_af; b = 0; P_vec = _B_force;      break;
+                      case 4: r = 0;        g = 1;    b = 1; P_vec = _B_wind;       break;
+                      case 5: r = 0;        g = 0;    b = 1; P_vec = _B_slipstream; break;
+                    }
+
+                    segments_[i].vec_vis[j]->mutable_color()->set_x(r);
+                    segments_[i].vec_vis[j]->mutable_color()->set_y(g);
+                    segments_[i].vec_vis[j]->mutable_color()->set_z(b);
+                    segments_[i].vec_vis[j]->mutable_startpoint()->set_x(P_start.X());
+                    segments_[i].vec_vis[j]->mutable_startpoint()->set_y(P_start.Y());
+                    segments_[i].vec_vis[j]->mutable_startpoint()->set_z(P_start.Z());
+                    segments_[i].vec_vis[j]->mutable_vector()->set_x(P_vec.X());
+                    segments_[i].vec_vis[j]->mutable_vector()->set_y(P_vec.Y());
+                    segments_[i].vec_vis[j]->mutable_vector()->set_z(P_vec.Z());
+                }
+
+                segments_[i].vector_vis_array_pub->Publish(segments_[i].vector_vis_array_msg);
+            }
+        }
+    }
+
+    if (n_bdy_>0) {
+        // iterate over body elements
+        for(int i=0; i<n_bdy_; i++){
+
+            // pose of reference link
+            GZ_ASSERT(bodies_[i].ref_link, "Link was NULL");
+#if GAZEBO_MAJOR_VERSION >= 9
+            ignition::math::Pose3d pose = bodies_[i].ref_link->WorldPose();
+#else
+            ignition::math::Pose3d pose = ignitionFromGazeboMath(bodies_[i].ref_link->GetWorldPose());
+#endif
+
+            V3D cp = pose.Pos() + pose.Rot().RotateVector(bodies_[i].cp);  // position of cp in world frame
+
+            bodies_[i].UpdateWind(cp);
+
+            // get velocity of cp, expressed in world frame
+#if GAZEBO_MAJOR_VERSION >= 9
+            V3D vel = bodies_[i].ref_link->WorldLinearVel(bodies_[i].cp);
+#else
+            V3D vel = ignitionFromGazeboMath(bodies_[i].ref_link->GetWorldLinearVel(bodies_[i].cp));
+#endif
+            vel = vel - bodies_[i].wind_cp;
+
+            // express forward, upward and spanwise vectors in world frame
+            V3D forward_i = pose.Rot().RotateVector(bodies_[i].fwd);
+            V3D upward_i = pose.Rot().RotateVector(bodies_[i].upwd);
+            V3D spanwise_i = forward_i.Cross(upward_i);
+            forward_i.Normalize();
+            upward_i.Normalize();
+            spanwise_i.Normalize();
+
+            // 'signed-quadratic' flow components in forward, upward and spanwise direction
+            double uu = vel.Dot(forward_i)*fabs(vel.Dot(forward_i));
+            double vv = vel.Dot(upward_i)*fabs(vel.Dot(upward_i));
+            double ww = vel.Dot(spanwise_i)*fabs(vel.Dot(spanwise_i));
+
+            // 'directional' drag coefficiens
+            double cd_x = bodies_[i].a_fus_xx * bodies_[i].cd_cyl_ax;
+            double cd_y = bodies_[i].a_fus_yy * bodies_[i].cd_cyl_lat;
+            double cd_z = bodies_[i].a_fus_zz * bodies_[i].cd_cyl_lat;
+
+            // calculate and apply drag fuselage drag force
+            V3D drag = -this->rho_/2.0*(forward_i*uu*cd_x + upward_i*vv*cd_y + spanwise_i*ww*cd_z);
+            bodies_[i].act_link->AddForceAtWorldPosition(drag, cp);
+
+            //visualization
+            if (bodies_[i].vector_vis_array_pub) {
+
+              // world to body frame
+              V3D _B_force = pose.Rot().RotateVectorReverse(drag);
+              V3D _B_wind = pose.Rot().RotateVectorReverse(bodies_[i].wind_cp);
+
+                float r,g,b;
+                V3D P_start = bodies_[i].cp;
+                V3D P_vec;
+
+                for(int j = 0; j < bodies_[i].vec_vis.size(); j++){
+
+                    switch (j) {
+                      case 0: r = 1; g = 0; b = 0; P_vec = _B_force; break;
+                      case 1: r = 0; g = 1; b = 1; P_vec = _B_wind;  break;
+                    }
+
+                    bodies_[i].vec_vis[j]->mutable_color()->set_x(r);
+                    bodies_[i].vec_vis[j]->mutable_color()->set_y(g);
+                    bodies_[i].vec_vis[j]->mutable_color()->set_z(b);
+                    bodies_[i].vec_vis[j]->mutable_startpoint()->set_x(P_start.X());
+                    bodies_[i].vec_vis[j]->mutable_startpoint()->set_y(P_start.Y());
+                    bodies_[i].vec_vis[j]->mutable_startpoint()->set_z(P_start.Z());
+                    bodies_[i].vec_vis[j]->mutable_vector()->set_x(P_vec.X());
+                    bodies_[i].vec_vis[j]->mutable_vector()->set_y(P_vec.Y());
+                    bodies_[i].vec_vis[j]->mutable_vector()->set_z(P_vec.Z());
+                }
+
+                bodies_[i].vector_vis_array_pub->Publish(bodies_[i].vector_vis_array_msg);
+            }
+        }
+    }
+
+}
diff --git a/rotors_gazebo_plugins/src/gazebo_propulsion_plugin.cpp b/rotors_gazebo_plugins/src/gazebo_propulsion_plugin.cpp
new file mode 100644
index 000000000..22b522550
--- /dev/null
+++ b/rotors_gazebo_plugins/src/gazebo_propulsion_plugin.cpp
@@ -0,0 +1,624 @@
+/*
+ * Copyright 2015 Fadri Furrer, ASL, ETH Zurich, Switzerland
+ * Copyright 2015 Michael Burri, ASL, ETH Zurich, Switzerland
+ * Copyright 2015 Mina Kamel, ASL, ETH Zurich, Switzerland
+ * Copyright 2015 Janosch Nikolic, ASL, ETH Zurich, Switzerland
+ * Copyright 2015 Markus Achtelik, ASL, ETH Zurich, Switzerland
+ *
+ * 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.
+ *
+ * Modifications by David Rohr, drohr@student.ethz.ch
+ *
+ */
+
+#include "rotors_gazebo_plugins/gazebo_propulsion_plugin.h"
+
+namespace gazebo {
+
+GazeboPropulsion::~GazeboPropulsion() {
+    update_connection_->~Connection();
+
+    for(int i=0; i<num_props_; i++){
+        delete[] propellers_[i].wind;
+    }
+
+    delete[] propellers_;
+    gzdbg<<"GazeboPropulsion plugin destructed\n";
+}
+
+void GazeboPropulsion::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf) {
+
+    gzdbg<<"Load started\n";
+
+    model_ = _model;
+    this->world_ = this->model_->GetWorld();
+
+    namespace_.clear();
+
+    if (_sdf->HasElement("robotNamespace"))
+        namespace_ = _sdf->GetElement("robotNamespace")->Get<std::string>();
+    else
+        gzerr<<"[gazebo_propulsion] Please specify a robotNamespace.\n";
+
+    node_handle_ = transport::NodePtr(new transport::Node());
+    node_handle_->Init();
+
+    /*
+    const YAML::Node nodeTest = YAML::Load("{name: Brewers, city: Milwaukee}");
+    std::string id_string;
+    gzdbg<<"yaml safe get: "<<SafeGet<std::string>(nodeTest, "name", &id_string)<<"\n";
+
+    YAML::Node primes = YAML::Load("1");
+    gzdbg<<"IsDefined: "<<primes.IsDefined()<<std::endl;
+    gzdbg<<"IsMap: "<<primes.IsMap()<<std::endl;
+    gzdbg<<"IsNull: "<<primes.IsNull()<<std::endl;
+    gzdbg<<"IsScalar: "<<primes.IsScalar()<<std::endl;
+    gzdbg<<"IsSequence: "<<primes.IsSequence()<<std::endl;
+    */
+
+    if (_sdf->HasElement("prop")){
+        sdf::ElementPtr _sdf_propeller = _sdf->GetElement("prop");
+
+        while (_sdf_propeller) {
+            _sdf_propeller = _sdf_propeller->GetNextElement("prop");
+            ++num_props_;
+        }
+
+        gzdbg<<"Found "<<num_props_<<" propellers for this plugin. \n";
+        propellers_ = new Propeller [num_props_];
+        _sdf_propeller = _sdf->GetElement("prop");
+
+        for (int idx=0; idx<num_props_; idx++) {
+            gzdbg<<"Processing propeller Nr: "<<idx<<" \n";
+
+            if (_sdf_propeller->HasElement("linkNameRef")) {
+                propellers_[idx].ref_link =  model_->GetLink(_sdf_propeller->Get<std::string>("linkNameRef"));
+
+                if (propellers_[idx].ref_link == NULL)
+                    gzthrow("[gazebo_propulsion] Couldn't find specified parent link: "<<_sdf_propeller->Get<std::string>("linkNameRef")<<"\n");
+
+            } else if (_sdf_propeller->HasElement("linkNameParent")) {  //for backward compatibility with older xacros
+                propellers_[idx].ref_link =  model_->GetLink(_sdf_propeller->Get<std::string>("linkNameParent"));
+
+                if (propellers_[idx].ref_link == NULL)
+                    gzthrow("[gazebo_propulsion] Couldn't find specified parent link: "<<_sdf_propeller->Get<std::string>("linkNameRef")<<"\n");
+            } else {
+                gzthrow("No parent link specified\n");
+            }
+
+            if (_sdf_propeller->HasElement("linkNameAct")) {
+                propellers_[idx].act_link =  model_->GetLink(_sdf_propeller->Get<std::string>("linkNameAct"));
+
+                if (propellers_[idx].act_link == NULL)
+                    gzthrow("Couldn't find specified link to act on: "<<_sdf_propeller->Get<std::string>("linkNameAct")<<"\n");
+
+            } else {
+                propellers_[idx].act_link = propellers_[idx].ref_link;
+                gzerr<<"Using linkNameRef for linkNameAct.\n";
+            }
+
+            if (_sdf_propeller->HasElement("cp"))
+                propellers_[idx].p_cp = _sdf_propeller->Get<V3D>("cp");
+            else
+                gzwarn<<"_sdf_propeller ["<<idx<<"] is missing 'cp' element \n";
+
+            if (_sdf_propeller->HasElement("axisPosRot"))
+                propellers_[idx].p_joint = _sdf_propeller->Get<V3D>("axisPosRot");
+            else
+                gzwarn<<"_sdf_propeller ["<<idx<<"] is missing 'axisPosRot' element \n";
+
+            propellers_[idx].p_joint.Normalize();
+
+            if (_sdf_propeller->HasElement("turnDir")) {
+                std::string turning_direction = _sdf_propeller->Get<std::string>("turnDir");
+
+                if (turning_direction == "cw")
+                    propellers_[idx].turning_direction = -1; // pos. thrust and axis of pos. rot. have opposite signs
+                else if (turning_direction == "ccw")
+                    propellers_[idx].turning_direction = 1;  // pos. thrust and axis of pos. rot. in same direction
+                else
+                    gzerr<<"[gazebo_propulsion] Please only use 'cw' or 'ccw' to specify turningDirection.\n";
+            } else {
+                gzwarn<<"Please specify a turning direction ('cw' or 'ccw'), defaults to ccw\n";
+            }
+
+            if (_sdf_propeller->HasElement("speedRefTopic")) {
+                propellers_[idx].omega_ref_subtopic = _sdf_propeller->GetElement("speedRefTopic")->Get<std::string>();
+                if(propellers_[idx].omega_ref_subtopic.empty())
+                    gzwarn<<"No speedRefTopic specified, will not generate thrust\n";
+            } else {
+                gzwarn<<"No speedRefTopic specified, will not generate thrust\n";
+            }
+
+            if (_sdf_propeller->HasElement("propParamsYAML")) {
+                std::string prop_paramsyaml = _sdf_propeller->GetElement("propParamsYAML")->Get<std::string>();
+                propellers_[idx].prop_params.LoadPropParamsYAML(prop_paramsyaml);
+            } else {
+                gzwarn<<"propeller ["<<idx<<"] is missing propeller parameters YAML file, "
+                     <<"using default parameters.\n";
+            }
+
+            if (_sdf_propeller->HasElement("tauUp"))
+                propellers_[idx].tau_p = _sdf_propeller->GetElement("tauUp")->Get<double>();
+            else
+                gzwarn<<"No time-constant for increasing throttle specified, using default \n";
+
+            if (_sdf_propeller->HasElement("tauDown"))
+                propellers_[idx].tau_n = _sdf_propeller->GetElement("tauDown")->Get<double>();
+            else
+                gzwarn<<"No time-constant for decreasing throttle specified, using default \n";
+
+            if (_sdf_propeller->HasElement("tauSpoolUp"))
+                propellers_[idx].tau_su = _sdf_propeller->GetElement("tauSpoolUp")->Get<double>();
+            else
+                gzwarn<<"No time-constant for spool-up specified, using default \n";
+
+            if (_sdf_propeller->HasElement("rdpsDead"))
+                propellers_[idx].omega_dead = _sdf_propeller->GetElement("rdpsDead")->Get<double>();
+            else
+                gzwarn<<"No prop speed dead-zone defined, using default \n";
+
+            if (_sdf_propeller->HasElement("rdpsMax"))
+                propellers_[idx].omega_max = _sdf_propeller->GetElement("rdpsMax")->Get<double>();
+            else
+                gzwarn<<"No max prop speed defined, using default \n";
+
+            // inertia tensor (assuming flat disk) expressed in propeller* frame
+            // propeller* frame: parent-fixed and x-axis aligned with prop axis
+            M3D inertia_prop = propellers_[idx].prop_params.mass *
+                    pow(propellers_[idx].prop_params.diameter/2,2) *
+                    M3D(0.5,0,0,0,0.25,0,0,0,0.25);
+
+            V3D rot_axis = -propellers_[idx].p_joint.Cross(V3D(1,0,0));
+            rot_axis.Normalize();
+            double rot_angle = acos(propellers_[idx].p_joint.Dot(V3D(1,0,0)));
+            M3D R_pa_pr; R_pa_pr.Axis(rot_axis,rot_angle);  // Maps from propeller* to parent frame
+
+            // inertia tensor (assuming flat disk) expressed in parent frame
+            propellers_[idx].inertia = R_pa_pr*(inertia_prop*(R_pa_pr.Inverse()));
+
+            if (_sdf_propeller->HasElement("slpstrTopic")) {
+                propellers_[idx].prop_slpstr_pubtopic = _sdf_propeller->GetElement("slpstrTopic")->Get<std::string>();
+                if(propellers_[idx].prop_slpstr_pubtopic.empty())
+                    gzwarn<<"No slisptream topic specified, won't publish.\n";
+            } else {
+                gzwarn<<"No slisptream topic specified, won't publish.\n";
+            }
+
+            if (_sdf_propeller->HasElement("visTopic")) {
+                propellers_[idx].vector_vis_array_topic = _sdf_propeller->GetElement("visTopic")->Get<std::string>();
+                if(propellers_[idx].vector_vis_array_topic.empty())
+                    gzdbg<<"No visualization topic specified, won't publish.\n";
+            } else {
+                gzdbg<<"No visualization topic specified, won't publish.\n";
+            }
+
+            propellers_[idx].vector_vis_array_msg.mutable_header()->mutable_stamp()->set_sec(0.0);
+            propellers_[idx].vector_vis_array_msg.mutable_header()->mutable_stamp()->set_nsec(0.0);
+            propellers_[idx].vector_vis_array_msg.mutable_header()->set_frame_id(propellers_[idx].ref_link->GetName());
+
+            // get winds
+            if (_sdf_propeller->HasElement("wind")) {
+                sdf::ElementPtr _sdf_wind= _sdf_propeller->GetElement("wind");
+
+                // get number of winds for this particular segment
+                while (_sdf_wind) {
+                    _sdf_wind = _sdf_wind->GetNextElement("wind");
+                    ++propellers_[idx].n_wind;
+                }
+
+                gzdbg<<"found "<<propellers_[idx].n_wind<<" wind(s) for propeller ["<<idx<<"]. \n";
+                _sdf_wind = _sdf_propeller->GetElement("wind");
+                propellers_[idx].wind = new Wind [propellers_[idx].n_wind];
+
+                for(int j=0; j<propellers_[idx].n_wind; j++){
+
+                    if(_sdf_wind->HasElement("topic")){
+                        propellers_[idx].wind[j].wind_topic = _sdf_wind->Get<std::string>("topic");
+                    } else {
+                        gzwarn<<"wind ["<<j<<"] of segment ["<<idx<<"] is missing 'topic' element \n";
+                    }
+
+                    _sdf_wind = _sdf_wind->GetNextElement("wind");
+                }
+
+
+                if(propellers_[idx].n_wind>0){
+                    propellers_[idx].wind_vis = propellers_[idx].vector_vis_array_msg.add_vector();
+                    propellers_[idx].wind_vis->set_ns(namespace_+"/prp_"+std::to_string(idx)+"/wind");
+                    propellers_[idx].wind_vis->set_id(0);
+                    propellers_[idx].wind_vis->mutable_scale()->set_x(0.025);
+                    propellers_[idx].wind_vis->mutable_scale()->set_y(0.05);
+                    propellers_[idx].wind_vis->mutable_scale()->set_z(0.05);
+                    propellers_[idx].wind_vis->mutable_color()->set_x(1.0);
+                    propellers_[idx].wind_vis->mutable_color()->set_y(1.0);
+                    propellers_[idx].wind_vis->mutable_color()->set_z(1.0);
+                }
+            }
+
+            // setup visualization message
+            for(int idx_vis=0; idx_vis<propellers_[idx].vec_vis.size(); idx_vis++){
+                propellers_[idx].vec_vis[idx_vis] = propellers_[idx].vector_vis_array_msg.add_vector();
+                propellers_[idx].vec_vis[idx_vis]->set_ns(namespace_+"/prp_"+std::to_string(idx));
+                propellers_[idx].vec_vis[idx_vis]->set_id(idx_vis);
+                propellers_[idx].vec_vis[idx_vis]->mutable_scale()->set_x(0.025);
+                propellers_[idx].vec_vis[idx_vis]->mutable_scale()->set_y(0.05);
+                propellers_[idx].vec_vis[idx_vis]->mutable_scale()->set_z(0.05);
+                propellers_[idx].vec_vis[idx_vis]->mutable_color()->set_x(1.0);
+                propellers_[idx].vec_vis[idx_vis]->mutable_color()->set_y(0.0);
+                propellers_[idx].vec_vis[idx_vis]->mutable_color()->set_z(0.0);
+            }
+
+            _sdf_propeller = _sdf_propeller->GetNextElement("prop");
+        }
+
+    } else {
+        gzerr<<"[gazebo_propulsion] Please specify propellers\n";
+    }
+
+    if (_sdf->HasElement("rhoAir"))
+        rho_air_ = _sdf->Get<double>("rhoAir");
+    else
+        gzwarn<<" missing rhoAir element, using default of 1.255 kg/m^3 \n";
+
+    // Listen to the update event. This event is broadcast everysimulation iteration.
+
+    update_connection_ = event::Events::ConnectWorldUpdateBegin(boost::bind(&GazeboPropulsion::OnUpdate, this));
+    gzdbg<<"Load completed, ConnectWorldUpdateBegin called\n";
+
+}
+
+// This gets called by the world update start event.
+void GazeboPropulsion::OnUpdate() {
+
+    sampling_time_ = world_->SimTime().Double() - prev_sim_time_;
+    prev_sim_time_ = world_->SimTime().Double();
+
+    if (!pubs_and_subs_created_) {
+        gazebo::transport::PublisherPtr connect_gazebo_to_ros_topic_pub =
+                node_handle_->Advertise<gz_std_msgs::ConnectGazeboToRosTopic>(
+                    "~/" + kConnectGazeboToRosSubtopic, 1);
+        gz_std_msgs::ConnectGazeboToRosTopic connect_gazebo_to_ros_topic_msg;
+        gzdbg<<"advertised ~/" + kConnectGazeboToRosSubtopic + "\n";
+
+        for (int idx = 0; idx<num_props_; idx++) {
+
+            if(!propellers_[idx].prop_slpstr_pubtopic.empty()){
+                propellers_[idx].prop_slpstr_pub = node_handle_->Advertise<gz_mav_msgs::PropulsionSlipstream>
+                        ("~/" + namespace_ + "/" + propellers_[idx].prop_slpstr_pubtopic, 1);
+                gzdbg<<"advertising: "<<"~/" + namespace_ + "/" + propellers_[idx].prop_slpstr_pubtopic<<"\n";
+            }
+
+            if(!propellers_[idx].vector_vis_array_topic.empty()){
+                propellers_[idx].vector_vis_array_pub = node_handle_->Advertise<gz_visualization_msgs::VisVectorArray>
+                        ("~/" + namespace_ + "/" + propellers_[idx].vector_vis_array_topic, 1);
+
+                connect_gazebo_to_ros_topic_msg.set_gazebo_topic("~/" + namespace_ + "/" + propellers_[idx].vector_vis_array_topic);
+                connect_gazebo_to_ros_topic_msg.set_ros_topic(namespace_ + "/" + propellers_[idx].vector_vis_array_topic);
+                connect_gazebo_to_ros_topic_msg.set_msgtype(gz_std_msgs::ConnectGazeboToRosTopic::VIS_VECTOR_ARRAY);
+                connect_gazebo_to_ros_topic_pub->Publish(connect_gazebo_to_ros_topic_msg, true);
+            }
+
+            if(!propellers_[idx].omega_ref_subtopic.empty()){
+                propellers_[idx].omega_ref_sub = node_handle_->Subscribe("~/" + namespace_ + "/" + propellers_[idx].omega_ref_subtopic,
+                                                                         &GazeboPropulsion::Propeller::PropSpeedCallback,
+                                                                         &propellers_[idx]);
+            }
+
+            for (int j=0; j<propellers_[idx].n_wind; j++){
+                propellers_[idx].wind[j].wind_sub_ = node_handle_->Subscribe("~/" + namespace_ + "/" + propellers_[idx].wind[j].wind_topic,
+                                                                         &GazeboPropulsion::Wind::Callback,
+                                                                         &propellers_[idx].wind[j]);
+                gzdbg<<"subscribing to: "<<"~/" + namespace_ + "/" + propellers_[idx].wind[j].wind_topic<<"\n";
+            }
+
+            propellers_[idx].speed_pub = node_handle_->Advertise<gazebo::msgs::Vector2d>(
+                        "~/" + namespace_ + "/speed_p" + std::to_string(idx), 1);
+        }
+
+        pubs_and_subs_created_ = true;
+    }
+
+    for (int idx = 0; idx<num_props_; idx++) {
+        propellers_[idx].dt = sampling_time_;
+        propellers_[idx].MotorDyn();            // update propeller speed
+
+        // pose of parent frame
+#if GAZEBO_MAJOR_VERSION >= 9
+        ignition::math::Pose3d pose_parent = propellers_[idx].ref_link->WorldPose();
+#else
+        ignition::math::Pose3d pose_parent = ignitionFromGazeboMath(propellers_[idx].ref_link->GetWorldPose());
+#endif
+
+        // Moments due to change in angular momentum of propeller, expressed in parent frame
+        // Fast spinning link is problematic due to aliasing effects in the physics engine
+        V3D H_Omega = propellers_[idx].ref_link->RelativeAngularVel();
+        V3D H_Omega_dot = (H_Omega - propellers_[idx].H_Omega_Prev_)/sampling_time_;// propellers_[idx].ref_link->RelativeAngularAccel();
+        propellers_[idx].H_Omega_Prev_ = H_Omega;
+
+        V3D H_moment_inertial = propellers_[idx].inertia*(H_Omega_dot + propellers_[idx].p_joint*propellers_[idx].omega_dot)
+                                + H_Omega.Cross(propellers_[idx].inertia*(H_Omega + propellers_[idx].p_joint*propellers_[idx].omega));
+
+        V3D H_moment_inertial_c1 = propellers_[idx].inertia*H_Omega_dot + H_Omega.Cross(propellers_[idx].inertia*H_Omega);
+        V3D H_moment_inertial_c2 = propellers_[idx].inertia*propellers_[idx].p_joint*propellers_[idx].omega_dot;
+        V3D H_moment_inertial_c3 = H_Omega.Cross(propellers_[idx].inertia*propellers_[idx].p_joint*propellers_[idx].omega);
+
+        if(isnan(H_moment_inertial.Length())) {
+          gzerr<<"H_moment_inertial"<<idx<<": NaN\n";
+        }
+
+        H_moment_inertial.Correct();
+        propellers_[idx].act_link->AddTorque(pose_parent.Rot().RotateVector(-H_moment_inertial));
+
+        V3D rotor_pos = pose_parent.Pos() + pose_parent.Rot().RotateVector(propellers_[idx].p_cp);
+        propellers_[idx].UpdateWind(rotor_pos);
+
+        // velocity of propeller hub
+#if GAZEBO_MAJOR_VERSION >= 9
+        V3D body_velocity = propellers_[idx].ref_link->WorldLinearVel(propellers_[idx].p_cp);
+#else
+        V3D body_velocity = ignitionFromGazeboMath(propellers_[idx].ref_link->GetWorldLinearVel(propellers_[idx].p_cp));
+#endif
+
+        // account for wind
+        body_velocity = body_velocity - propellers_[idx].wind_cp;
+
+        // propeller axis expressed in world frame, in positive rot direction
+        V3D joint_axis = pose_parent.Rot().RotateVector(propellers_[idx].p_joint);
+        joint_axis.Normalize();
+
+        double rps = propellers_[idx].omega/(2*M_PI);
+
+        V3D forward = joint_axis*propellers_[idx].turning_direction*(rps>=0 ? 1.0 : -1.0);   // positive thrust direction (for symmetric propellers, we want enable two-way operation)
+
+        // resolve local airspeed into axial (V_inf_a) and radial (V_inf_r) component
+        V3D body_velocity_radial = body_velocity - body_velocity.Dot(forward) * forward;
+        V3D body_velocity_axial = body_velocity.Dot(forward) * forward;
+        //V3D rotor_pos = propellers_[idx].ref_link->WorldPose().Pos() + pose_parent.Rot().RotateVector(propellers_[idx].p_cp);  // link position expressed in world frame
+
+        double V_inf_a = body_velocity.Dot(forward);    // axial component of relative flow wrt propeller disk (no wind assumed)
+        double V_inf_a_clmpd = std::max(V_inf_a,0.0);   // treat reverse flow as static case
+        double V_inf_r = body_velocity_radial.Length(); // radial component of relative flow wrt propeller disk (no wind assumed)
+
+        double J   = V_inf_a_clmpd/(std::max(std::abs(rps),0.1) * propellers_[idx].prop_params.diameter);    // Advance ratio (set minimum rps to prevent division by zero...)
+
+        // ---- Thrust ----
+
+        //double C_T = propellers_[idx].prop_params.k_t*J + propellers_[idx].prop_params.k_t0;
+        //double thrust = rho_air_*pow(rps,2)*pow(propellers_[idx].prop_params.diameter,4)*C_T;
+        double thrust = rho_air_*std::abs(rps)*pow(propellers_[idx].prop_params.diameter,3)*
+                (propellers_[idx].prop_params.k_t*V_inf_a_clmpd +
+                 std::abs(rps)*propellers_[idx].prop_params.diameter*propellers_[idx].prop_params.k_t0);
+
+        if(!isnan(thrust)) {
+          //propellers_[idx].ref_link->AddLinkForce(thrust*pose_parent.Rot().RotateVectorReverse(forward), propellers_[idx].p_cp);
+          propellers_[idx].act_link->AddForceAtWorldPosition(thrust*forward,rotor_pos);
+        } else {
+          gzerr<<"thrust_"<<idx<<": NaN\n";
+        }
+
+        // ---- Normal force ----
+
+        V3D hub_force_ = -std::abs(rps*2*M_PI) * propellers_[idx].prop_params.rotor_drag_coefficient * body_velocity_radial;
+        if(!isnan(hub_force_.Length())) {
+          //propellers_[idx].ref_link->AddLinkForce(pose_parent.Rot().RotateVectorReverse(hub_force_), propellers_[idx].p_cp);
+          propellers_[idx].act_link->AddForceAtWorldPosition(hub_force_,rotor_pos);
+        } else {
+          gzerr<<"hub_force_"<<idx<<": NaN\n";
+        }
+
+        // ---- Drag torque ----
+
+        double drag_torque = -rho_air_*std::abs(rps)*pow(propellers_[idx].prop_params.diameter,4)*
+                (V_inf_a_clmpd*propellers_[idx].prop_params.k_q +
+                 std::abs(rps)*propellers_[idx].prop_params.diameter*propellers_[idx].prop_params.k_q0)*
+                (rps>=0 ? 1.0 : -1.0);
+        V3D drag_torque_ = joint_axis * drag_torque;
+        if(!isnan(drag_torque_.Length())) {
+          //propellers_[idx].ref_link->AddTorque(drag_torque_);
+          propellers_[idx].act_link->AddTorque(drag_torque_);
+        } else {
+          gzerr<<"drag_torque"<<idx<<": NaN\n";
+        }
+
+        // ---- Rolling moment ----
+
+        V3D rolling_moment = -propellers_[idx].turning_direction*std::abs(rps*2*M_PI) *
+                propellers_[idx].prop_params.rolling_moment_coefficient *
+                body_velocity_radial;
+        if(!isnan(rolling_moment.Length())) {
+          //propellers_[idx].ref_link->AddTorque(rolling_moment);
+          propellers_[idx].act_link->AddTorque(rolling_moment);
+        } else {
+          gzerr<<"rolling_moment"<<idx<<": NaN\n";
+        }
+
+        // ---- Propeller slipstream ----
+
+        thrust = std::max(thrust,0.0);  //Currently, breaking (neg. thrust can result from above calc) is ignored for wake modelling since this flight regime is assumed to be entered rarely and handlig breaking would require a separate case (i.e. jf>=1, k_w would need to be set to a value smaller than one -> in breaking state, the streamlines are diverging opposite to the direction of travel, hence the induced velocity behind the prop decreases monotonically (in contrast to the non-braking case, where the induced velocity first increases behind the prop before exhibiting loss-driven decay further downstream).
+        double disk_area = pow(propellers_[idx].prop_params.diameter,2)/4*M_PI;
+        double w = 0.5*(-V_inf_a_clmpd + sqrt(pow(V_inf_a_clmpd,2)+2*thrust/(rho_air_*disk_area)));  // induced velocity at disk, w>=0
+        double V_disk_a = V_inf_a+w;    //airflow velocity at propeller disk, w>=0
+
+        V3D w_ds_a_;                // axial induced velocity
+        V3D w_ds_r_;                // radial induced velocity
+        V3D w_dir(0.0,0.0,0.0);     // propeller hub -> wake end
+        V3D w_end(0.0,0.0,0.0);     // induced velocity at end of wake
+        V3D w_disk(0.0,0.0,0.0);    // induced velocity at disk
+
+        double k_w;
+        double m_dot_clmpd = disk_area*(V_inf_a_clmpd + w)*rho_air_; //>=0
+
+        if (V_disk_a>0) {
+            // i.e. wake extends beyond prop...
+
+            double jf = V_inf_a/V_disk_a;     // jet flow parameter (cf 'm' in selig paper)
+
+            if (jf<=0)                        // hover & descent(V_inf_a<=0)
+                k_w = 1.0;
+            else if (jf>=0.75)                // full cruise
+                k_w = 1.8;
+            else
+                k_w = 1.0 + jf/0.75*0.8;
+
+            // induced velocity at disk in axial direction, accounting for losses (k_w)
+            w_ds_a_ = -k_w*w*forward;
+
+            // induced velocity at disk in radial direction
+            // w_ds_r_ = -hub_force_/hub_force_.Length() * ignition::math::clamp(hub_force_.Length()/m_dot_clmpd*pow(std::abs(rps*2*M_PI)/max_rot_velocity_,0.25), 0.0, 0.8*body_velocity_radial.Length());
+            w_ds_r_ = V3D(0,0,0);
+            // double hub_vel_rel = w_ds_r_.Length()/V_inf_r; // debug: so far no check if hub_vel_rel < 1 ... should it be <1?
+
+            w_disk = w_ds_a_+w_ds_r_; // downstream values already at disk for simplification
+
+            // get wake
+            if (V_inf_a >= 0) {
+                // case I: No reverse free-stream, declare wake as terminated if induced velocity decayed to 1/8 of its initial value
+                double t_end = log(8.0)/propellers_[idx].prop_params.d_flow;
+                w_dir = -t_end*body_velocity + (7.0/8.0)*(w_ds_a_+w_ds_r_)/propellers_[idx].prop_params.d_flow;
+                w_end = (w_ds_a_+w_ds_r_)/8.0;
+
+            } else {
+                // case II: Flow in wake changes direction (relative to propeller) after some time due to reverse free-stream
+                double t_end = log(-w_ds_a_.Length()/V_inf_a)/propellers_[idx].prop_params.d_flow;
+                w_dir = -t_end*body_velocity + (1.0-exp(-propellers_[idx].prop_params.d_flow*t_end))*(w_ds_a_+w_ds_r_)/propellers_[idx].prop_params.d_flow;
+                w_end = (w_ds_a_+w_ds_r_)*exp(-propellers_[idx].prop_params.d_flow*t_end);
+            }
+
+        } else {
+            // case III: wake does not extend beyond prop, reverse flow through prop disk, no wake modelled
+            w_dir = V3D(0.0,0.0,0.0);
+            w_end = V3D(0.0,0.0,0.0);
+        }
+
+        // ---- Fill propeller slipstream message ----
+
+        if(propellers_[idx].prop_slpstr_pub){
+
+            propulsion_slipstream_msg_.mutable_rotor_pos()->set_x(rotor_pos.X());
+            propulsion_slipstream_msg_.mutable_rotor_pos()->set_y(rotor_pos.Y());
+            propulsion_slipstream_msg_.mutable_rotor_pos()->set_z(rotor_pos.Z());
+
+            propulsion_slipstream_msg_.mutable_ind_vel_disk()->set_x(w_disk.X());
+            propulsion_slipstream_msg_.mutable_ind_vel_disk()->set_y(w_disk.Y());
+            propulsion_slipstream_msg_.mutable_ind_vel_disk()->set_z(w_disk.Z());
+
+            propulsion_slipstream_msg_.mutable_ind_vel_end()->set_x(w_end.X());
+            propulsion_slipstream_msg_.mutable_ind_vel_end()->set_y(w_end.Y());
+            propulsion_slipstream_msg_.mutable_ind_vel_end()->set_z(w_end.Z());
+
+            propulsion_slipstream_msg_.mutable_wake_dir()->set_x(w_dir.X());
+            propulsion_slipstream_msg_.mutable_wake_dir()->set_y(w_dir.Y());
+            propulsion_slipstream_msg_.mutable_wake_dir()->set_z(w_dir.Z());
+
+            propulsion_slipstream_msg_.mutable_timestamp()->set_sec((int)0);
+            propulsion_slipstream_msg_.mutable_timestamp()->set_nsec((int)0);
+
+            propulsion_slipstream_msg_.set_k_w(k_w);
+            propulsion_slipstream_msg_.set_l_a(0.0);
+            propulsion_slipstream_msg_.set_l_p(0.0);
+            propulsion_slipstream_msg_.set_prop_diam(propellers_[idx].prop_params.diameter);
+
+            propellers_[idx].prop_slpstr_pub->Publish(propulsion_slipstream_msg_);
+        }
+
+        if(propellers_[idx].vector_vis_array_pub){
+
+            float r,g,b;
+            V3D P_start = propellers_[idx].p_cp;//V3D(-0.025,0,0.15); //
+            V3D P_vec;
+
+            for(int idx_vis=0; idx_vis<propellers_[idx].vec_vis.size(); idx_vis++){
+
+                switch (idx_vis) {
+                case 0:
+                    r=1;g=0;b=0;
+                    P_vec = pose_parent.Rot().RotateVectorReverse(thrust*forward/10);
+                    //P_vec = H_moment_inertial_1;
+                    break;
+                case 1:
+                    r=0;g=1;b=0;
+                    P_vec = pose_parent.Rot().RotateVectorReverse(hub_force_/10);
+                    //P_vec = H_moment_inertial_2;
+                    break;
+                case 2:
+                    r=0;g=0;b=1;
+                    P_vec = pose_parent.Rot().RotateVectorReverse(drag_torque_);
+                    //P_vec = propellers_[idx].p_joint*propellers_[idx].omega_dot;
+                    //P_vec = H_moment_inertial_3;
+                    break;
+                case 3:
+                    r=1;g=1;b=0; //yellow
+                    P_vec = pose_parent.Rot().RotateVectorReverse(H_moment_inertial);
+                    //P_vec = pose_parent.Rot().RotateVectorReverse(rolling_moment);
+                    //P_vec = propellers_[idx].p_joint*propellers_[idx].omega_dot;
+                    //P_vec = H_moment_inertial;
+                    break;
+                case 4:
+                    r=1;g=0;b=1; //violet
+                    //P_vec = pose_parent.Rot().RotateVectorReverse(H_Omega_dot);
+                    P_vec = pose_parent.Rot().RotateVectorReverse(H_moment_inertial_c1);
+                    //P_vec = pose_parent.Rot().RotateVectorReverse(w_dir);
+                    //P_vec = 10*V3D(propellers_[idx].inertia(0,0),propellers_[idx].inertia(1,0),propellers_[idx].inertia(2,0));
+                    //P_vec = 50*propellers_[idx].inertia*propellers_[idx].p_joint;
+                    break;
+                case 5:
+                    r=0;g=1;b=1; //cyan
+                    //P_vec = pose_parent.Rot().RotateVectorReverse(H_Omega);
+                    P_vec = pose_parent.Rot().RotateVectorReverse(H_moment_inertial_c2); P_start = propellers_[idx].p_cp + pose_parent.Rot().RotateVectorReverse(H_moment_inertial_c1);
+                    //P_vec = pose_parent.Rot().RotateVectorReverse(w_disk);
+                    //P_vec = 10*V3D(propellers_[idx].inertia(0,1),propellers_[idx].inertia(1,1),propellers_[idx].inertia(2,1));
+                    //P_vec = 50*propellers_[idx].inertia*V3D(0,1,0);
+                    break;
+                case 6:
+                    r=1;g=1;b=1; //white
+                    P_vec = pose_parent.Rot().RotateVectorReverse(H_moment_inertial_c3); P_start = propellers_[idx].p_cp + pose_parent.Rot().RotateVectorReverse(H_moment_inertial_c1 + H_moment_inertial_c2);
+                    //P_vec = -H_moment_inertial;
+                    //P_vec = 10*V3D(propellers_[idx].inertia(0,2),propellers_[idx].inertia(1,2),propellers_[idx].inertia(2,2));
+                    //P_vec = 50*propellers_[idx].inertia*V3D(0.2,0.0,1);
+                    break;
+                }
+
+                propellers_[idx].vec_vis[idx_vis]->mutable_color()->set_x(r);
+                propellers_[idx].vec_vis[idx_vis]->mutable_color()->set_y(g);
+                propellers_[idx].vec_vis[idx_vis]->mutable_color()->set_z(b);
+                propellers_[idx].vec_vis[idx_vis]->mutable_startpoint()->set_x(P_start.X());
+                propellers_[idx].vec_vis[idx_vis]->mutable_startpoint()->set_y(P_start.Y());
+                propellers_[idx].vec_vis[idx_vis]->mutable_startpoint()->set_z(P_start.Z());
+                propellers_[idx].vec_vis[idx_vis]->mutable_vector()->set_x(P_vec.X());
+                propellers_[idx].vec_vis[idx_vis]->mutable_vector()->set_y(P_vec.Y());
+                propellers_[idx].vec_vis[idx_vis]->mutable_vector()->set_z(P_vec.Z());
+            }
+
+            if(propellers_[idx].n_wind>0){
+                V3D _B_wind = pose_parent.Rot().RotateVectorReverse(propellers_[idx].wind_cp);
+                propellers_[idx].wind_vis->mutable_color()->set_x(0.0);
+                propellers_[idx].wind_vis->mutable_color()->set_y(1.0);
+                propellers_[idx].wind_vis->mutable_color()->set_z(1.0);
+                propellers_[idx].wind_vis->mutable_startpoint()->set_x(P_start.X());
+                propellers_[idx].wind_vis->mutable_startpoint()->set_y(P_start.Y());
+                propellers_[idx].wind_vis->mutable_startpoint()->set_z(P_start.Z());
+                propellers_[idx].wind_vis->mutable_vector()->set_x(_B_wind.X());
+                propellers_[idx].wind_vis->mutable_vector()->set_y(_B_wind.Y());
+                propellers_[idx].wind_vis->mutable_vector()->set_z(_B_wind.Z());
+            }
+
+            propellers_[idx].vector_vis_array_pub->Publish(propellers_[idx].vector_vis_array_msg);
+        }
+    }
+
+    update_counter_++;
+}
+
+GZ_REGISTER_MODEL_PLUGIN(GazeboPropulsion);
+}
diff --git a/rotors_gazebo_plugins/src/gazebo_wind_beta_plugin.cpp b/rotors_gazebo_plugins/src/gazebo_wind_beta_plugin.cpp
new file mode 100644
index 000000000..e487ba2d6
--- /dev/null
+++ b/rotors_gazebo_plugins/src/gazebo_wind_beta_plugin.cpp
@@ -0,0 +1,863 @@
+/*
+ * Copyright 2015 Fadri Furrer, ASL, ETH Zurich, Switzerland
+ * Copyright 2015 Michael Burri, ASL, ETH Zurich, Switzerland
+ * Copyright 2015 Mina Kamel, ASL, ETH Zurich, Switzerland
+ * Copyright 2015 Janosch Nikolic, ASL, ETH Zurich, Switzerland
+ * Copyright 2015 Markus Achtelik, ASL, ETH Zurich, Switzerland
+ * Copyright 2016 Geoffrey Hunter <gbmhunter@gmail.com>
+ *
+ * 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 "rotors_gazebo_plugins/gazebo_wind_beta_plugin.h"
+
+#include <fstream>
+#include <math.h>
+
+#include "ConnectGazeboToRosTopic.pb.h"
+
+namespace gazebo {
+
+GazeboWindBetaPlugin::~GazeboWindBetaPlugin() {
+    delete[] wind_queries_;
+    delete[] wind_layers_;
+    delete[] layer_index_alt_incr_;
+
+    for (int iix=0; iix<nk_x_; iix++) {
+        for (int iiy=0; iiy<nk_y_; iiy++) {
+            delete[] C_ij_[iix][iiy];
+            delete[] rand_phase_[iix][iiy];
+        }
+        delete[] C_ij_[iix];
+        delete[] rand_phase_[iix];
+    }
+    delete[] rand_phase_;
+    delete[] C_ij_;
+}
+
+void GazeboWindBetaPlugin::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf) {
+    if (kPrintOnPluginLoad) {
+        gzdbg << __FUNCTION__ << "() called." << std::endl;
+    }
+
+    // Store the pointer to the model.
+    model_ = _model;
+    world_ = model_->GetWorld();
+
+    //==============================================//
+    //========== READ IN PARAMS FROM SDF ===========//
+    //==============================================//
+
+    if (_sdf->HasElement("robotNamespace"))
+        namespace_ = _sdf->GetElement("robotNamespace")->Get<std::string>();
+    else
+        gzerr << "[gazebo_wind_beta_plugin] Please specify a robotNamespace.\n";
+
+    // Create Gazebo Node.
+    node_handle_ = gazebo::transport::NodePtr(new transport::Node());
+
+    // Initialize with default namespace (typically /gazebo/default/).
+    node_handle_->Init();
+
+    if (_sdf->HasElement("findWind")) {
+
+        sdf::ElementPtr _sdf_findWind = _sdf->GetElement("findWind");
+        sdf::ElementPtr _sdf_query = _sdf_findWind->GetElement("query");
+
+        while (_sdf_query) {
+            _sdf_query = _sdf_query->GetNextElement("query");
+            ++n_query_;
+        }
+
+        gzdbg<<"Found "<<n_query_<<" points to calculate wind \n";
+        wind_queries_ = new WindQuery [n_query_];
+
+        _sdf_query = _sdf_findWind->GetElement("query");
+
+        for (int i=0; i<n_query_; i++) {
+            if (_sdf_query->HasElement("linkName")) {
+                std::string link_name = _sdf_query->Get<std::string>("linkName");
+                wind_queries_[i].link = model_->GetLink(link_name);
+
+                if (!wind_queries_[i].link) {
+                    gzerr<<"Couldn't find specified link \"" << link_name << "\", won't publish.\n";
+                    wind_queries_[i].sdf_valid = false;
+                }
+
+            } else if (_sdf_query->HasElement("worldPos")) {
+                wind_queries_[i].pos_ned = _sdf_query->Get<ignition::math::Vector3d>("worldPos");
+
+            } else {
+                gzwarn<<"Don't know where to query wind, won't publish. \n";
+                wind_queries_[i].sdf_valid = false;
+            }
+
+            if (_sdf_query->HasElement("windTopic")) {
+                wind_queries_[i].wind_topic = _sdf_query->Get<std::string>("windTopic");
+
+            } else {
+                gzwarn<<"Please specify windTopic, won't publish wind data. \n";
+                wind_queries_[i].sdf_valid = false;
+            }
+
+            getSdfParam<std::string>(_sdf_query, "frameId", wind_queries_[i].frame_id, wind_queries_[i].frame_id);
+
+            _sdf_query = _sdf_query->GetNextElement("query");
+        }
+    }
+
+    if (_sdf->HasElement("windLayer")) {
+
+        sdf::ElementPtr _sdf_windLayer = _sdf->GetElement("windLayer");
+        sdf::ElementPtr _sdf_layer = _sdf_windLayer->GetElement("layer");
+
+        while (_sdf_layer) {
+            _sdf_layer = _sdf_layer->GetNextElement("layer");
+            ++n_layer_;
+        }
+
+        gzdbg<<"Found "<<n_layer_<<" wind layers \n";
+        wind_layers_ = new WindLayer [n_layer_];
+        layer_index_alt_incr_ = new int [n_layer_];
+        _sdf_layer = _sdf_windLayer->GetElement("layer");
+
+        for (int i=0; i<n_layer_; i++) {
+            getSdfParam<double>(_sdf_layer, "altitude", wind_layers_[i].altitude,
+                                wind_layers_[i].altitude);
+            /*
+               getSdfParam<double>(_sdf_layer, "altUpper", wind_layers_[i].alt_upper,
+                                   wind_layers_[i].alt_upper);
+               getSdfParam<bool>(_sdf_layer, "useRelAlt", wind_layers_[i].rel_alt,
+                                 wind_layers_[i].rel_alt);
+               */
+
+            getSdfParam<V3D>(_sdf_layer, "windMeanNED", wind_layers_[i].wind_mean,
+                             wind_layers_[i].wind_mean);
+            /*
+               getSdfParam<double>(_sdf_layer, "windSpeedVariance", wind_layers_[i].wind_speed_variance,
+                                   wind_layers_[i].wind_speed_variance);
+               getSdfParam<bool>(_sdf_layer, "gustSpeedMean", wind_layers_[i].rel_alt,
+                                 wind_layers_[i].rel_alt);
+               getSdfParam<bool>(_sdf_layer, "gustSpeedVariance", wind_layers_[i].rel_alt,
+                                 wind_layers_[i].rel_alt);
+               getSdfParam<bool>(_sdf_layer, "windNedDir", wind_layers_[i].rel_alt,
+                                 wind_layers_[i].rel_alt);
+               getSdfParam<bool>(_sdf_layer, "gustNedDir", wind_layers_[i].rel_alt,
+                                 wind_layers_[i].rel_alt);
+               getSdfParam<bool>(_sdf_layer, "gust", wind_layers_[i].rel_alt,
+                                 wind_layers_[i].rel_alt);
+               */
+
+            _sdf_layer = _sdf_layer->GetNextElement("layer");
+        }
+
+        // sort layers, ascending altitude
+        std::sort(wind_layers_,wind_layers_+n_layer_,GazeboWindBetaPlugin::SortByAltitude);
+    }
+
+    if (_sdf->HasElement("lambdaMin"))
+        lambda_min_ = _sdf->Get<double>("lambdaMin");
+
+    if (_sdf->HasElement("lengthScale"))
+        L_ = _sdf->Get<double>("lengthScale");
+
+    if (_sdf->HasElement("fourierNNN")) {
+        ignition::math::Vector3i fourierNNN = _sdf->Get<ignition::math::Vector3i>("fourierNNN");
+        nk_x_ = std::max(1,fourierNNN.X());
+        nk_y_ = std::max(1,fourierNNN.Y());
+        nk_z_ = std::max(1,fourierNNN.Z());
+
+    } else {
+        nk_x_ = 15;
+        nk_y_ = 15;
+        nk_z_ = 15;
+    }
+
+    // ensure odd number of components
+    if(nk_x_%2==0)
+        nk_x_+=1;
+
+    if(nk_y_%2==0)
+        nk_y_+=1;
+
+    if(nk_z_%2==0)
+        nk_z_+=1;
+
+    if (_sdf->HasElement("sigma"))
+        sigma_ = _sdf->Get<double>("sigma");
+
+    if (_sdf->HasElement("trnspVelTurbNED"))
+        trnsp_vel_turb_ = _sdf->Get<V3D>("trnspVelTurbNED");
+
+    if (_sdf->HasElement("customWindFieldPath")) {
+        gzdbg << "[gazebo_wind_plugin] Using custom wind field from text file.\n";
+        // Get the wind field text file path, read it and save data.
+        std::string custom_wind_field_path;
+        getSdfParam<std::string>(_sdf, "customWindFieldPath", custom_wind_field_path,
+                                 custom_wind_field_path);
+
+        getSdfParam<double>(_sdf, "windFieldXOffset", wf_offset.X(), wf_offset.X());
+        getSdfParam<double>(_sdf, "windFieldYOffset", wf_offset.Y(), wf_offset.Y());
+        getSdfParam<double>(_sdf, "windFieldZOffset", wf_offset.Z(), wf_offset.Z());
+
+        ReadCustomWindFieldCSV(custom_wind_field_path);
+        ProcessCustomWindField();
+    }
+
+    // Set up Fourier Simulation
+    std::random_device rd;
+    std::mt19937 gen(rd());
+    //std::uniform_real_distribution<> r_phase(0.0, 2*M_PI);
+    std::normal_distribution<> re(0,1.0/sqrt(2));   // complex variance = var(re)+var(im) needs= 1
+    std::normal_distribution<> im(0,1.0/sqrt(2));
+
+    C_ij_ = new M3C** [nk_x_];
+    rand_phase_ = new V3C** [nk_x_];
+
+    for (int iix=0; iix<nk_x_; iix++) {
+        C_ij_[iix] = new M3C* [nk_y_];
+        rand_phase_[iix] = new V3C* [nk_y_];
+
+        for (int iiy=0; iiy<nk_y_; iiy++) {
+            C_ij_[iix][iiy] = new M3C [nk_z_];
+            rand_phase_[iix][iiy] = new V3C [nk_z_];
+        }
+    }
+
+    dk_x_ = 2*M_PI/lambda_min_/nk_x_;
+    dk_y_ = 2*M_PI/lambda_min_/nk_y_;
+    dk_z_ = 2*M_PI/lambda_min_/nk_z_;
+
+    for (int iix=0; iix<nk_x_; iix++) {
+        for (int iiy=0; iiy<nk_y_; iiy++) {
+            for (int iiz=0; iiz<nk_z_; iiz++) {
+                V3D k((iix-(nk_x_-1)/2)*dk_x_, (iiy-(nk_y_-1)/2)*dk_y_, (iiz-(nk_z_-1)/2)*dk_z_);
+
+                /*
+                rand_phase_[iix][iiy][iiz].Set(std::exp((std::complex<double>)1i*r_phase(gen)),
+                                               std::exp((std::complex<double>)1i*r_phase(gen)),
+                                               std::exp((std::complex<double>)1i*r_phase(gen)));
+                */
+                rand_phase_[iix][iiy][iiz].Set((std::complex<double>)re(gen) + (std::complex<double>)1i*im(gen),
+                                               (std::complex<double>)re(gen) + (std::complex<double>)1i*im(gen),
+                                               (std::complex<double>)re(gen) + (std::complex<double>)1i*im(gen));
+
+                // only use k's inside spherical volume, otherwise set contribution to zero
+                if (k.Length()>0 && k.Length()<M_PI/lambda_min_) {
+                    GazeboWindBetaPlugin::SpectralTensorIsoIncDec(C_ij_[iix][iiy][iiz], k, L_, sigma_);
+                    C_ij_[iix][iiy][iiz] = C_ij_[iix][iiy][iiz]*sqrt(dk_x_*dk_y_*dk_z_);
+
+                } else {
+                    C_ij_[iix][iiy][iiz].Set(0,0,0,0,0,0,0,0,0);
+                }
+            }
+        }
+    }
+
+    /*
+     gzdbg<<"C_ij_[2][3][4]: \n"; PrintMatrix(C_ij_[2][3][4]); gzdbg<<"\n";
+     gzdbg<<"C_ij_[8][7][13]: \n"; PrintMatrix(C_ij_[8][7][13]); gzdbg<<"\n";
+     gzdbg<<"C_ij_[0][1][13]: \n"; PrintMatrix(C_ij_[0][1][13]); gzdbg<<"\n";
+     gzdbg<<"dk_x_: "<<dk_x_<<" dk_y_: "<<dk_y_<<" dk_z_: "<<dk_z_<<"\n";
+     gzdbg<<"lambdaMin: "<<lambda_min_<<" lengthScale: "<<L_<<" sigma: "<<sigma_<<"\n";
+     */
+
+    // Listen to the update event. This event is broadcast every
+    // simulation iteration.
+    update_connection_ = event::Events::ConnectWorldUpdateBegin(boost::bind(&GazeboWindBetaPlugin::OnUpdate, this, _1));
+}
+
+void GazeboWindBetaPlugin::PrintMatrix(M3C M) {
+
+    gzdbg<<std::real(M(0,0))<<" "<<std::real(M(0,1))<<" "<<std::real(M(0,2))<<"\n";
+    gzdbg<<std::real(M(1,0))<<" "<<std::real(M(1,1))<<" "<<std::real(M(1,2))<<"\n";
+    gzdbg<<std::real(M(2,0))<<" "<<std::real(M(2,1))<<" "<<std::real(M(2,2))<<"\n";
+}
+
+// This gets called by the world update start event.
+void GazeboWindBetaPlugin::OnUpdate(const common::UpdateInfo& _info) {
+
+    if (!pubs_and_subs_created_) {
+        CreatePubsAndSubs();
+        pubs_and_subs_created_ = true;
+    }
+
+    for (int iiq=0; iiq<n_query_; iiq++) {
+
+        ignition::math::Pose3d link_pose = wind_queries_[iiq].link->WorldPose();
+        V3D pos = link_pose.Pos();
+
+        // wind component from layered model (from xacro)
+        V3D vel_lyr = V3D(0,0,0);
+
+        // wind component from turbulent field
+        V3C vel_turb = V3C(0,0,0);
+        V3C dvel_x_turb = V3C(0,0,0);
+        V3C dvel_y_turb = V3C(0,0,0);
+        V3C dvel_z_turb = V3C(0,0,0);
+
+        // wind component from custom field (from file)
+        V3D vel_cust_field = V3D(0,0,0);
+        V3D dvdx_cust_field = V3D(0,0,0);
+        V3D dvdy_cust_field = V3D(0,0,0);
+        V3D dvdz_cust_field = V3D(0,0,0);
+
+        // Layered mean velocity field from xacro (uniform in x-y)
+        if (n_layer_>=2) {
+            for (int iil=0; iil<n_layer_-1; iil++) {
+                // below lowest specified layer
+                if (pos.Z()<wind_layers_[iil].altitude) {
+                    if (pos.Z()<=0)
+                        vel_lyr = V3D(0,0,0);
+                    else
+                        vel_lyr = wind_layers_[iil].wind_mean*pos.Z()/wind_layers_[iil].altitude;
+                    break;
+                }
+
+                // in range of specified layers
+                if (pos.Z()>=wind_layers_[iil].altitude && pos.Z()<wind_layers_[iil+1].altitude) {
+                    vel_lyr = wind_layers_[iil].wind_mean +
+                            (wind_layers_[iil+1].wind_mean-wind_layers_[iil].wind_mean)*
+                            (pos.Z()-wind_layers_[iil].altitude)/(wind_layers_[iil+1].altitude-wind_layers_[iil].altitude);
+                    break;
+                }
+
+                // on or above top layer
+                if (iil==n_layer_-2)
+                    vel_lyr = wind_layers_[iil+1].wind_mean;
+            }
+
+        } else if (n_layer_==1) {
+            if (pos.Z()<=0)
+                vel_lyr = V3D(0,0,0);
+            else if (pos.Z()<=wind_layers_[0].altitude)
+                vel_lyr = wind_layers_[0].wind_mean*pos.Z()/wind_layers_[0].altitude;
+            else
+                vel_lyr = wind_layers_[0].wind_mean;
+
+        } else {
+            vel_lyr = V3D(0,0,0);
+        }
+
+        // ToDo: more robust trafo NED to world
+        vel_lyr.Y() = -vel_lyr.Y();
+        vel_lyr.Z() = -vel_lyr.Z();
+
+        // Turbulence field (Fourier simulation)
+#if GAZEBO_MAJOR_VERSION >= 9
+        V3D pos_off = -trnsp_vel_turb_*world_->SimTime().Double();
+#else
+        V3D pos_off = -trnsp_vel_turb_*world_->GetSimTime().Double();
+#endif
+
+        V3D pos_q = pos + pos_off;
+        V3D pos_q_dx = pos + pos_off + V3D(1,0,0);
+        V3D pos_q_dy = pos + pos_off + V3D(0,1,0);
+        V3D pos_q_dz = pos + pos_off + V3D(0,0,1);
+
+        for (int iix=0; iix<nk_x_; iix++) {
+            for (int iiy=0; iiy<nk_y_; iiy++) {
+                for (int iiz=0; iiz<nk_z_; iiz++) {
+                    V3D k((iix-(nk_x_-1)/2)*dk_x_, (iiy-(nk_y_-1)/2)*dk_y_, (iiz-(nk_z_-1)/2)*dk_z_);
+                    V3C arg = C_ij_[iix][iiy][iiz]*rand_phase_[iix][iiy][iiz];
+                    vel_turb = vel_turb + std::exp((std::complex<double>)-1i*k.Dot(pos_q))*arg;
+                    dvel_x_turb = dvel_x_turb + std::exp((std::complex<double>)-1i*k.Dot(pos_q_dx))*arg;
+                    dvel_y_turb = dvel_y_turb + std::exp((std::complex<double>)-1i*k.Dot(pos_q_dy))*arg;
+                    dvel_z_turb = dvel_z_turb + std::exp((std::complex<double>)-1i*k.Dot(pos_q_dz))*arg;
+                }
+            }
+        }
+
+        // ToDo: check again own derivations...
+        // ToDo: modify turbulence according to local turbulent kinetic energy trb_kin_nrg_...
+        double sr2 = sqrt(2);
+        vel_turb *= sr2;
+        dvel_x_turb *= sr2;
+        dvel_y_turb *= sr2;
+        dvel_z_turb *= sr2;
+
+        // scale turbulence in proximity of ground (z=0)
+        double red_hrz_z = sqrt(ignition::math::clamp(pos.Z()/10.0,0.0,1.0));
+        double red_vrt_z = sqrt(ignition::math::clamp(pos.Z()/20.0,0.0,1.0));
+
+        // Custom mean velocity field
+        if (wind_is_valid_) {
+
+            // Calculate the x, y index of the grid points with x, y-coordinate
+            // just smaller than or equal to aircraft x, y position.
+            std::size_t x_inf = floor((pos.X() - wf_offset.X() - min_x_) / res_x_);
+            std::size_t y_inf = floor((pos.Y() - wf_offset.Y() - min_y_) / res_y_);
+            std::size_t z_inf = floor((pos.Z() - wf_offset.Z() - min_z_) / res_z_);
+
+            // Calculate the x, y index of the grid points with x, y-coordinate just
+            // greater than the aircraft x, y position.
+            std::size_t x_sup = x_inf + 1u;
+            std::size_t y_sup = y_inf + 1u;
+            std::size_t z_sup = z_inf + 1u;
+
+            // Save in an array the x, y index of each of the eight grid points
+            // enclosing the aircraft.
+            constexpr unsigned int n_vertices = 8;
+            std::size_t idx_x[n_vertices] = {x_inf,x_inf,x_inf,x_inf,x_sup,x_sup,x_sup,x_sup}; // {x_inf, x_inf, x_sup, x_sup, x_inf, x_inf, x_sup, x_sup};
+            std::size_t idx_y[n_vertices] = {y_inf,y_inf,y_sup,y_sup,y_inf,y_inf,y_sup,y_sup}; // {y_inf, y_inf, y_inf, y_inf, y_sup, y_sup, y_sup, y_sup};
+            std::size_t idx_z[n_vertices] = {z_inf,z_sup,z_inf,z_sup,z_inf,z_sup,z_inf,z_sup}; // {z_inf, z_sup, z_sup, z_inf, z_inf, z_sup, z_sup, z_inf};
+
+            // Check if aircraft is out of wind field or not, and act accordingly.
+            if (x_inf >= 0u && y_inf >= 0u && z_inf >= 0u &&
+                    x_sup <= (n_x_ - 2u) && y_sup <= (n_y_ - 2u) && z_sup <= (n_z_ - 2u)) {
+
+                // Extract the wind velocities corresponding to each vertex.
+                V3D wind_at_vertices[n_vertices];
+                V3D dwfdx_at_vertices[n_vertices];
+                V3D dwfdy_at_vertices[n_vertices];
+                V3D dwfdz_at_vertices[n_vertices];
+
+                for (std::size_t i = 0u; i < n_vertices; ++i) {
+                    wind_at_vertices[i].X() = wf_[idx_x[i] + idx_y[i] * n_x_ + idx_z[i] * n_x_ * n_y_].X();
+                    wind_at_vertices[i].Y() = wf_[idx_x[i] + idx_y[i] * n_x_ + idx_z[i] * n_x_ * n_y_].Y();
+                    wind_at_vertices[i].Z() = wf_[idx_x[i] + idx_y[i] * n_x_ + idx_z[i] * n_x_ * n_y_].Z();
+
+                    dwfdx_at_vertices[i] = dwfdx_[idx_x[i] + idx_y[i] * n_x_ + idx_z[i] * n_x_ * n_y_];
+                    dwfdy_at_vertices[i] = dwfdy_[idx_x[i] + idx_y[i] * n_x_ + idx_z[i] * n_x_ * n_y_];
+                    dwfdz_at_vertices[i] = dwfdz_[idx_x[i] + idx_y[i] * n_x_ + idx_z[i] * n_x_ * n_y_];
+                }
+
+                // Extract the relevant coordinate of every point needed for trilinear
+                double interpolation_points[14] = { min_z_+idx_z[0]*res_z_,
+                                                    min_z_+idx_z[1]*res_z_,
+                                                    min_z_+idx_z[2]*res_z_,
+                                                    min_z_+idx_z[3]*res_z_,
+                                                    min_z_+idx_z[4]*res_z_,
+                                                    min_z_+idx_z[5]*res_z_,
+                                                    min_z_+idx_z[6]*res_z_,
+                                                    min_z_+idx_z[7]*res_z_,
+                                                    min_y_+idx_y[0]*res_y_,
+                                                    min_y_+idx_y[2]*res_y_,
+                                                    min_y_+idx_y[4]*res_y_,
+                                                    min_y_+idx_y[6]*res_y_,
+                                                    min_x_+idx_x[0]*res_x_,
+                                                    min_x_+idx_x[4]*res_x_ };
+
+                // Interpolate wind velocity and and gradients at aircraft position.
+
+                vel_cust_field  = TrilinearInterpolation(pos-wf_offset, wind_at_vertices, interpolation_points);
+                dvdx_cust_field = TrilinearInterpolation(pos-wf_offset, dwfdx_at_vertices, interpolation_points);
+                dvdy_cust_field = TrilinearInterpolation(pos-wf_offset, dwfdy_at_vertices, interpolation_points);
+                //dvdz_cust_field = TrilinearInterpolation(pos-wf_offset, dwfdz_at_vertices, interpolation_points);
+            }
+        }
+
+        // Superpose everything
+        wind_queries_[iiq].wind_msg.mutable_pos_ned()->set_x(std::real(pos.X()));
+        wind_queries_[iiq].wind_msg.mutable_pos_ned()->set_y(std::real(pos.Y()));
+        wind_queries_[iiq].wind_msg.mutable_pos_ned()->set_z(std::real(pos.Z()));
+
+        wind_queries_[iiq].wind_msg.mutable_wind_ned()->set_x(std::real(vel_turb.X())*red_hrz_z+vel_lyr.X()+vel_cust_field.X());
+        wind_queries_[iiq].wind_msg.mutable_wind_ned()->set_y(std::real(vel_turb.Y())*red_hrz_z+vel_lyr.Y()+vel_cust_field.Y());
+        wind_queries_[iiq].wind_msg.mutable_wind_ned()->set_z(std::real(vel_turb.Z())*red_vrt_z+vel_lyr.Z()+vel_cust_field.Z());
+
+        wind_queries_[iiq].wind_msg.mutable_wind_grad_ned()->set_xx(std::real(dvel_x_turb.X()-vel_turb.X())*red_hrz_z+dvdx_cust_field.X());
+        wind_queries_[iiq].wind_msg.mutable_wind_grad_ned()->set_xy(std::real(dvel_y_turb.X()-vel_turb.X())*red_hrz_z+dvdy_cust_field.X());
+        wind_queries_[iiq].wind_msg.mutable_wind_grad_ned()->set_xz(std::real(dvel_z_turb.X()-vel_turb.X())*red_hrz_z+dvdz_cust_field.X());
+        wind_queries_[iiq].wind_msg.mutable_wind_grad_ned()->set_yx(std::real(dvel_x_turb.Y()-vel_turb.Y())*red_hrz_z+dvdx_cust_field.Y());
+        wind_queries_[iiq].wind_msg.mutable_wind_grad_ned()->set_yy(std::real(dvel_y_turb.Y()-vel_turb.Y())*red_hrz_z+dvdy_cust_field.Y());
+        wind_queries_[iiq].wind_msg.mutable_wind_grad_ned()->set_yz(std::real(dvel_z_turb.Y()-vel_turb.Y())*red_hrz_z+dvdz_cust_field.Y());
+        wind_queries_[iiq].wind_msg.mutable_wind_grad_ned()->set_zx(std::real(dvel_x_turb.Z()-vel_turb.Z())*red_vrt_z+dvdx_cust_field.Z());
+        wind_queries_[iiq].wind_msg.mutable_wind_grad_ned()->set_zy(std::real(dvel_y_turb.Z()-vel_turb.Z())*red_vrt_z+dvdy_cust_field.Z());
+        wind_queries_[iiq].wind_msg.mutable_wind_grad_ned()->set_zz(std::real(dvel_z_turb.Z()-vel_turb.Z())*red_vrt_z+dvdz_cust_field.Z());
+
+        wind_queries_[iiq].wind_pub->Publish(wind_queries_[iiq].wind_msg);
+
+        if (counter%100==0) {
+            gzdbg<<"turb: "<<std::real(vel_turb.X())<<" "<<std::real(vel_turb.Y())<<" "<<std::real(vel_turb.Z())<<"\n";
+            gzdbg<<"mean: "<<vel_lyr.X()<<" "<<vel_lyr.Y()<<" "<<vel_lyr.Z()<<"\n";
+            gzdbg<<"alt: "<<pos.Z()<<"\n";
+            counter = 0;
+        }
+    }
+
+    counter++;
+
+}
+
+void GazeboWindBetaPlugin::CreatePubsAndSubs() {
+    // Create temporary "ConnectGazeboToRosTopic" publisher and message.
+    gazebo::transport::PublisherPtr connect_gazebo_to_ros_topic_pub =
+            node_handle_->Advertise<gz_std_msgs::ConnectGazeboToRosTopic>(
+                "~/" + kConnectGazeboToRosSubtopic, 1);
+
+    gz_std_msgs::ConnectGazeboToRosTopic connect_gazebo_to_ros_topic_msg;
+
+    // ============================================ //
+    // ========= WRENCH STAMPED MSG SETUP ========= //
+    // ============================================ //
+    /*
+  wind_force_pub_ = node_handle_->Advertise<gz_geometry_msgs::WrenchStamped>(
+      "~/" + namespace_ + "/" + wind_force_pub_topic_, 1);
+
+  // connect_gazebo_to_ros_topic_msg.set_gazebo_namespace(namespace_);
+  connect_gazebo_to_ros_topic_msg.set_gazebo_topic("~/" + namespace_ + "/" +
+                                                   wind_force_pub_topic_);
+  connect_gazebo_to_ros_topic_msg.set_ros_topic(namespace_ + "/" +
+                                                wind_force_pub_topic_);
+  connect_gazebo_to_ros_topic_msg.set_msgtype(
+      gz_std_msgs::ConnectGazeboToRosTopic::WRENCH_STAMPED);
+  connect_gazebo_to_ros_topic_pub->Publish(connect_gazebo_to_ros_topic_msg,
+                                           true);
+  */
+    // ============================================ //
+    // ========== WIND SPEED MSG SETUP ============ //
+    // ============================================ //
+
+    for (int i=0; i<n_query_; i++) {
+        wind_queries_[i].wind_pub = node_handle_->Advertise<gz_mav_msgs::WindSpeedBeta>(
+                    "~/" + namespace_ + "/" + wind_queries_[i].wind_topic, 1);
+
+        connect_gazebo_to_ros_topic_msg.set_gazebo_topic("~/" + namespace_ + "/" +
+                                                         wind_queries_[i].wind_topic);
+        connect_gazebo_to_ros_topic_msg.set_ros_topic(namespace_ + "/" +
+                                                      wind_queries_[i].wind_topic);
+        // connect_gazebo_to_ros_topic_msg.set_msgtype(
+        //     gz_std_msgs::ConnectGazeboToRosTopic::WIND_SPEED);
+        // connect_gazebo_to_ros_topic_pub->Publish(connect_gazebo_to_ros_topic_msg,
+        //                                          true);
+    }
+}
+
+void  GazeboWindBetaPlugin::KarmanEnergySpectrum(double &E, V3D k, double L, double sigma) {
+
+    // Evaluates Karman energy spectrum at spatial frequency k
+    //
+    // Ref: J.Mann, "The spatial structure of neutral atmospheric surface-layer turbulence",
+    //      J. Fluid. Mech. (1994), vol. 273, p.146-p.147
+    //
+    // 1.4528 = 55/9*Gamma(5/6)/Gamma(1/3)/sqrt(pi)
+
+    double LK2 = L*L*k.Length()*k.Length();
+    E=1.4528*L*sigma*sigma*LK2*LK2/pow(1+LK2,17.0/6.0);
+
+}
+
+void  GazeboWindBetaPlugin::SpectralTensorIsoInc(M3D &Phi, V3D k, double L, double sigma) {
+
+    // Returns spectral tensor Phi (Fourier transform of covariance tensor) at spatial frequency k,
+    // using the Karman spectral energy.
+    //
+    // Ref: J.Mann, "The spatial structure of neutral atmospheric surface-layer turbulence",
+    //      J. Fluid. Mech. (1994), vol. 273, p.146-p.147
+
+    double E;
+    GazeboWindBetaPlugin::KarmanEnergySpectrum(E, k, L, sigma);
+
+    Phi.Set( k[1]*k[1]+k[2]*k[2], -k[0]*k[1], -k[0]*k[2],
+            -k[1]*k[0], k[0]*k[0]+k[2]*k[2], -k[1]*k[2],
+            -k[2]*k[0], -k[2]*k[1], k[0]*k[0]+k[1]*k[1]);
+
+    Phi = E/(4*M_PI*pow(k.Length(),4))*Phi;
+}
+
+void  GazeboWindBetaPlugin::SpectralTensorIsoIncDec(M3C &A, V3D k, double L, double sigma) {
+
+    // Returns decomposition A of spectral tensor Phi (Fourier transform of covariance tensor)
+    // at spatial frequency k, using the Karman spectral energy. A'A=Phi
+    //
+    // Ref: J.Mann, "Wind field simulation", Prop. Engng. Mech. (1998), vol. 13, p.280
+
+    double E;
+    GazeboWindBetaPlugin::KarmanEnergySpectrum(E, k, L, sigma);
+
+    A.Set(0, k[2], -k[1],
+            -k[2], 0, k[0],
+            k[1], -k[0], 0);
+
+    A = sqrt(E/(4*M_PI))/(k.Length()*k.Length())*A;
+}
+
+/*
+void GazeboWindBetaPlugin::LonPsdKarman(double * omega, double * phi, double n, double l, double s){
+
+     double A = 2*s*s*l/M_PI;
+     for (int i = 0; i < n; i++) {
+          double B = 1.339*l*omega[i];
+          phi[i] = A * 1/(pow(1+B*B,5/6));
+     }
+}
+
+void GazeboWindBetaPlugin::LatVertPsdKarman(double * omega, double * phi, double n, double l, double s){
+
+     double A = s*s*l/M_PI;
+     for (int i = 0; i < n; i++) {
+          double B = 1.339*l*omega[i];
+          phi[i] = A * (1+8/3*B*B)/(pow(1+B*B,11/6));
+     }
+}
+*/
+
+void GazeboWindBetaPlugin::ProcessCustomWindField() {
+
+    if (wind_is_valid_) {
+
+        for (int iix=0; iix<n_x_-1; iix++) {
+            for (int iiy=0; iiy<n_y_-1; iiy++) {
+                for (int iiz=0; iiz<n_z_-1; iiz++) {
+
+                    int i0  = iix+iiy*n_x_+iiz*n_x_*n_y_;
+                    int idx = (iix+1)+iiy*n_x_+iiz*n_x_*n_y_;
+                    int idy = iix+(iiy+1)*n_x_+iiz*n_x_*n_y_;
+                    int idz = iix+iiy*n_x_+(iiz+1)*n_x_*n_y_;
+
+                    dwfdx_.push_back((wf_[idx]-wf_[i0])/res_x_);
+                    dwfdy_.push_back((wf_[idy]-wf_[i0])/res_y_);
+                    dwfdz_.push_back((wf_[idz]-wf_[i0])/res_z_);
+                }
+            }
+        }
+    }
+}
+
+void GazeboWindBetaPlugin::ReadCustomWindFieldCSV(std::string& custom_wind_field_path) {
+
+    std::ifstream fin;
+    fin.open(custom_wind_field_path);
+
+    if (fin.is_open()) {
+
+        std::vector<float> x_coord;
+        std::vector<float> y_coord;
+        std::vector<float> z_coord;
+        int n_grid = 0;
+        double data[11];
+        char comma;
+        bool stop = false;
+        bool start = true;
+        bool got_n_x = false;
+        bool got_n_y = false;
+        bool got_n_z = false;
+
+        // Ignore header
+        fin.ignore(128, '\n');
+
+        for (;;) {
+
+            // read 11 numbers in current line and break on error
+            for (int iid=0; iid<10; iid++) {
+                fin >> data[iid] >> comma;
+                if (fin.fail() || fin.eof())
+                    stop = true;
+            }
+
+            fin >> data[10];
+
+            if (fin.fail() || fin.eof())
+                stop = true;
+
+            if (stop)
+                break;
+
+            ++n_grid;
+
+            wf_.push_back(V3D(data[1],data[2],data[3]));
+            trb_kin_nrg_.push_back(data[5]);
+
+            if (start) {
+                min_x_ = data[8];
+                min_y_ = data[9];
+                min_z_ = data[10];
+                n_x_ = 1;
+                n_y_ = 1;
+                n_z_ = 1;
+                start = false;
+
+            } else {
+                if (!got_n_x && data[8]!=min_x_) {
+                    ++n_x_;
+                    x_coord.push_back(data[8]);
+
+                } else if (data[8]==min_x_) {
+                    got_n_x=true;
+                    if (!got_n_y && data[9]!=min_y_) {
+                        ++n_y_;
+                        y_coord.push_back(data[9]);
+
+                    } else if (data[9]==min_y_) {
+                        got_n_y=true;
+                        if (!got_n_z && data[10]!=min_z_) {
+                            ++n_z_;
+                            z_coord.push_back(data[10]);
+
+                        } else {
+                            got_n_z=true;
+                        }
+                    }
+                }
+            }
+
+            // ignore rest of line
+            fin.ignore(128, '\n');
+        }
+
+        gzerr<<"Custom wind field: nx="<<n_x_<<" | ny="<<n_y_<<" | nz="<<n_z_<<"\n";
+        gzerr<<"Custom wind field: min_x="<<min_x_<<" | min_y="<<min_y_<<" | min_z="<<min_z_<<"\n";
+
+        if (n_x_>1 && n_y_>1 && n_z_>1) {
+            wind_is_valid_ = true;
+            res_x_ = x_coord[1]-x_coord[0];
+            res_y_ = y_coord[1]-y_coord[0];
+            res_z_ = z_coord[1]-z_coord[0];
+
+        } else {
+            gzerr<<"Custom wind field not valid, too few grid points (min 2x2x2).\n";
+        }
+
+    } else {
+        gzerr<<"Could not open custom wind field *.csv file. Check file permissions.\n";
+    }
+
+}
+
+/*
+void GazeboWindBetaPlugin::ReadCustomWindField(std::string& custom_wind_field_path) {
+    std::ifstream fin;
+    fin.open(custom_wind_field_path);
+    if (fin.is_open()) {
+        std::string data_name;
+        float data;
+        // Read the line with the variable name.
+        while (fin >> data_name) {
+            // Save data on following line into the correct variable.
+            if (data_name == "min_x:") {
+                fin >> min_x_;
+            } else if (data_name == "min_y:") {
+                fin >> min_y_;
+            } else if (data_name == "n_x:") {
+                fin >> n_x_;
+            } else if (data_name == "n_y:") {
+                fin >> n_y_;
+            } else if (data_name == "res_x:") {
+                fin >> res_x_;
+            } else if (data_name == "res_y:") {
+                fin >> res_y_;
+            } else if (data_name == "vertical_spacing_factors:") {
+                while (fin >> data) {
+                    vertical_spacing_factors_.push_back(data);
+                    if (fin.peek() == '\n') break;
+                }
+            } else if (data_name == "bottom_z:") {
+                while (fin >> data) {
+                    bottom_z_.push_back(data);
+                    if (fin.peek() == '\n') break;
+                }
+            } else if (data_name == "top_z:") {
+                while (fin >> data) {
+                    top_z_.push_back(data);
+                    if (fin.peek() == '\n') break;
+                }
+            } else if (data_name == "u:") {
+                while (fin >> data) {
+                    u_.push_back(data);
+                    if (fin.peek() == '\n') break;
+                }
+            } else if (data_name == "v:") {
+                while (fin >> data) {
+                    v_.push_back(data);
+                    if (fin.peek() == '\n') break;
+                }
+            } else if (data_name == "w:") {
+                while (fin >> data) {
+                    w_.push_back(data);
+                    if (fin.peek() == '\n') break;
+                }
+            } else {
+                // If invalid data name, read the rest of the invalid line,
+                // publish a message and ignore data on next line. Then resume reading.
+                std::string restOfLine;
+                getline(fin, restOfLine);
+                gzerr << " [gazebo_wind_plugin] Invalid data name '" << data_name << restOfLine <<
+                         "' in custom wind field text file. Ignoring data on next line.\n";
+                fin.ignore(std::numeric_limits<std::streamsize>::max(), '\n');
+            }
+        }
+        fin.close();
+
+        gzdbg << "[gazebo_wind_plugin] Successfully read custom wind field from text file.\n";
+
+
+
+    } else {
+        gzerr << "[gazebo_wind_plugin] Could not open custom wind field text file.\n";
+    }
+
+
+
+}
+*/
+
+ignition::math::Vector3d GazeboWindBetaPlugin::LinearInterpolation(
+        double position, ignition::math::Vector3d * values, double* points) const {
+    ignition::math::Vector3d value = values[0] + (values[1] - values[0]) /
+            (points[1] - points[0]) * (position - points[0]);
+    return value;
+}
+
+ignition::math::Vector3d GazeboWindBetaPlugin::BilinearInterpolation(
+        double* position, ignition::math::Vector3d * values, double* points) const {
+    ignition::math::Vector3d intermediate_values[2] = { LinearInterpolation(
+                                                        position[1], &(values[0]), &(points[0])),
+                                                        LinearInterpolation(
+                                                        position[1], &(values[2]), &(points[2])) };
+    ignition::math::Vector3d value = LinearInterpolation(
+                position[0], intermediate_values, &(points[4]));
+    return value;
+}
+
+ignition::math::Vector3d GazeboWindBetaPlugin::TrilinearInterpolation(
+        ignition::math::Vector3d link_position, ignition::math::Vector3d * values, double* points) const {
+    double position[3] = {link_position.X(),link_position.Y(),link_position.Z()};
+
+    /*
+    gzerr<<"Trilipol:   Pos: "<<position[0]<<","<<position[1]<<","<<position[2]<<"\n";
+    std::stringstream ss;
+    ss<<"         Points: ";
+    for(int i=0; i<14; i++){
+        ss<<points[i]<<", ";
+    }
+    gzerr<<ss.str()<<"\n";
+    gzerr<<"           Wind: "<<values[0].X()<<", "<<values[1].X()<<", "<<values[2].X()<<", "<<values[3].X()<<", "<<values[4].X()<<", "<<values[5].X()<<", "<<values[6].X()<<", "<<values[7].X()<<"\n";
+    gzerr<<"                 "<<values[0].Y()<<", "<<values[1].Y()<<", "<<values[2].Y()<<", "<<values[3].Y()<<", "<<values[4].Y()<<", "<<values[5].Y()<<", "<<values[6].Y()<<", "<<values[7].Y()<<"\n";
+    gzerr<<"                 "<<values[0].Z()<<", "<<values[1].Z()<<", "<<values[2].Z()<<", "<<values[3].Z()<<", "<<values[4].Z()<<", "<<values[5].Z()<<", "<<values[6].Z()<<", "<<values[7].Z()<<"\n";
+
+    gzerr<<"                 nx="<<n_x_<<" | ny="<<n_y_<<" | nz="<<n_z_<<"\n";
+    gzerr<<"                 min_x="<<min_x_<<" | min_y="<<min_y_<<" | min_z="<<min_z_<<"\n";
+    gzerr<<"                 res_x="<<res_x_<<" | res_y="<<res_y_<<" | res_z="<<res_z_<<"\n";
+    */
+
+    ignition::math::Vector3d intermediate_values[4] = { LinearInterpolation(
+                                                        position[2], &(values[0]), &(points[0])),
+                                                        LinearInterpolation(
+                                                        position[2], &(values[2]), &(points[2])),
+                                                        LinearInterpolation(
+                                                        position[2], &(values[4]), &(points[4])),
+                                                        LinearInterpolation(
+                                                        position[2], &(values[6]), &(points[6])) };
+    ignition::math::Vector3d value = BilinearInterpolation(
+                &(position[0]), intermediate_values, &(points[8]));
+    /*
+    gzerr<<"                 vx="<<value.X()<<" | vy="<<value.Y()<<" | vz="<<value.Z()<<"\n";
+    */
+
+    return value;
+}
+
+GZ_REGISTER_MODEL_PLUGIN(GazeboWindBetaPlugin);
+
+}  // namespace gazebo