diff --git a/rotors_description/urdf/component_snippets.xacro b/rotors_description/urdf/component_snippets.xacro
index fcf0164..050b260 100644
--- a/rotors_description/urdf/component_snippets.xacro
+++ b/rotors_description/urdf/component_snippets.xacro
@@ -954,8 +954,92 @@
     accelerometer_turn_on_bias_sigma="0">
     <inertia ixx="0.00001" ixy="0.0" ixz="0.0" iyy="0.00001" iyz="0.0" izz="0.00001" />
     <origin xyz="0 0 0" rpy="0 0 0" />
-  </xacro:imu_plugin_macro>
-</xacro:macro>
+   </xacro:imu_plugin_macro>
+  </xacro:macro>
+
+  <!-- VI-Sensor Macro Without Visual Part -->
+  <xacro:macro name="vi_sensor_macro_without_visual" params="namespace parent_link enable_cameras enable_depth enable_ground_truth *origin">
+    <!-- Vi Sensor Link -->
+    <link name="${namespace}/vi_sensor_link" />
+    <joint name="${namespace}_vi_sensor_joint" type="fixed">
+      <xacro:insert_block name="origin" />
+      <parent link="${parent_link}" />
+      <child link="${namespace}/vi_sensor_link" />
+    </joint>
+    <!-- Cameras -->
+    <xacro:if value="${enable_cameras}">
+      <!-- Insert stereo pair. -->
+      <xacro:vi_sensor_stereo_camera_macro
+        namespace="${namespace}" parent_link="${namespace}/vi_sensor_link"
+        frame_rate="30.0" origin_offset_x="0.015" baseline_y="${0.055*2}"
+        origin_offset_z="0.0065" max_range="30.0">
+      </xacro:vi_sensor_stereo_camera_macro>
+    </xacro:if>
+
+    <!-- Depth Sensor -->
+    <xacro:if value="${enable_depth}">
+      <xacro:vi_sensor_depth_macro
+        namespace="${namespace}" parent_link="${namespace}/vi_sensor_link"
+        camera_suffix="depth" frame_rate="30.0" max_range="10.0">
+        <origin xyz="0.015 0.055 0.0065" rpy="0 0 0" />
+      </xacro:vi_sensor_depth_macro>
+    </xacro:if>
+
+    <!-- Groundtruth -->
+    <xacro:if value="${enable_ground_truth}">
+      <!-- Odometry Sensor -->
+      <xacro:odometry_plugin_macro
+        namespace="${namespace}/ground_truth"
+        odometry_sensor_suffix=""
+        parent_link="${namespace}/vi_sensor_link"
+        pose_topic="pose"
+        pose_with_covariance_topic="pose_with_covariance"
+        position_topic="position"
+        transform_topic="transform"
+        odometry_topic="odometry"
+        parent_frame_id="world"
+        child_frame_id="${namespace}/base_link"
+        mass_odometry_sensor="0.00001"
+        measurement_divisor="1"
+        measurement_delay="0"
+        unknown_delay="0.0"
+        noise_normal_position="0 0 0"
+        noise_normal_quaternion="0 0 0"
+        noise_normal_linear_velocity="0 0 0"
+        noise_normal_angular_velocity="0 0 0"
+        noise_uniform_position="0 0 0"
+        noise_uniform_quaternion="0 0 0"
+        noise_uniform_linear_velocity="0 0 0"
+        noise_uniform_angular_velocity="0 0 0"
+        enable_odometry_map="false"
+        odometry_map=""
+        image_scale="">
+        <inertia ixx="0.00001" ixy="0.0" ixz="0.0" iyy="0.00001" iyz="0.0" izz="0.00001" /> <!-- [kg m^2] [kg m^2] [kg m^2] [kg m^2] [kg m^2] [kg m^2] -->
+        <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0" />
+      </xacro:odometry_plugin_macro>
+    </xacro:if>
+
+   <!-- Mount an MPU-9250 IMU. -->
+    <xacro:imu_plugin_macro
+      namespace="${namespace}"
+      imu_suffix=""
+      parent_link="${parent_link}"
+      imu_topic="imu"
+      measurement_delay="0"
+      measurement_divisor="1"
+      mass_imu_sensor="0.00001"
+      gyroscope_noise_density="0.000175"
+      gyroscope_random_walk="0.0105"
+      gyroscope_bias_correlation_time="1000.0"
+      gyroscope_turn_on_bias_sigma= "0.09"
+      accelerometer_noise_density="0.003"
+      accelerometer_random_walk="0.18"
+      accelerometer_bias_correlation_time="300.0"
+      accelerometer_turn_on_bias_sigma="0.588">
+      <inertia ixx="0.00001" ixy="0.0" ixz="0.0" iyy="0.00001" iyz="0.0" izz="0.00001" />
+      <origin xyz="0 0 0" rpy="0 0 0" />
+    </xacro:imu_plugin_macro>
+  </xacro:macro>
 
   <xacro:macro name="default_gps" params="namespace parent_link">
     <!-- Default GPS. -->
diff --git a/rotors_description/urdf/crazyflie2_base.xacro b/rotors_description/urdf/crazyflie2_base.xacro
index 890e75b..6aaf094 100644
--- a/rotors_description/urdf/crazyflie2_base.xacro
+++ b/rotors_description/urdf/crazyflie2_base.xacro
@@ -60,4 +60,16 @@
       wait_to_record_bag="$(arg wait_to_record_bag)" />
   </xacro:if>
 
+  <!-- Mount a VI-sensor in front of the Firefly. -->
+  <xacro:if value="$(arg enable_vi_sensor)">
+     <xacro:vi_sensor_macro_without_visual
+      namespace="${namespace}/vi_sensor"
+      parent_link="${namespace}/base_link"
+      enable_cameras="true"
+      enable_depth="true"
+      enable_ground_truth="true">
+      <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0" />
+     </xacro:vi_sensor_macro_without_visual>
+  </xacro:if>
+
 </robot>
diff --git a/rotors_gazebo/launch/crazyflie2_internal_model_controller_vi_sensor.launch b/rotors_gazebo/launch/crazyflie2_internal_model_controller_vi_sensor.launch
new file mode 100644
index 0000000..3980115
--- /dev/null
+++ b/rotors_gazebo/launch/crazyflie2_internal_model_controller_vi_sensor.launch
@@ -0,0 +1,76 @@
+<?xml version="1.0"?>
+<launch>
+  <arg name="mav_name" default="crazyflie2"/>
+  <!-- Pay attention that the world sampling time is line with the one used in the
+       controller algorithm-->
+  <arg name="world_name" default="basic"/>
+  <arg name="enable_logging" default="false" />
+  <arg name="enable_ground_truth" default="true" />
+  <arg name="log_file" default="$(arg mav_name)" />
+  <arg name="paused" value="true"/>
+  <arg name="debug" default="false"/>
+  <arg name="gui" default="true"/>
+  <arg name="enable_vi_sensor" default="true"/>
+  <!-- The following line causes gzmsg and gzerr messages to be printed to the console
+      (even when Gazebo is started through roslaunch) -->
+  <arg name="verbose" default="false"/>
+  <!-- Enables the Internal Model Controller disabling the Mellinger and the Position ones -->
+  <arg name="enable_internal_model_controller" default="true"/>
+  <arg name="debug_rviz" default="false" />
+  <arg name="config" default="true" />
+  <arg unless="$(arg debug_rviz)" name="launch_prefix" value="" />
+  <arg     if="$(arg debug_rviz)" name="launch_prefix" value="gdb --ex run --args" />
+  <arg unless="$(arg config)" name="command_args" value="" />
+  <arg     if="$(arg config)" name="command_args" value="-d $(find rotors_gazebo)/rviz/crazyflie_vi_sensor.rviz" />
+
+  <!-- The following lines simulate the world in Gazebo. The physic engine properties
+        are set up in the file "basic_crazyflie.world" file -->
+  <env name="GAZEBO_MODEL_PATH" value="${GAZEBO_MODEL_PATH}:$(find rotors_gazebo)/models"/>
+  <env name="GAZEBO_RESOURCE_PATH" value="${GAZEBO_RESOURCE_PATH}:$(find rotors_gazebo)/models"/>
+  <include file="$(find gazebo_ros)/launch/empty_world.launch">
+    <arg name="world_name" value="$(find rotors_gazebo)/worlds/$(arg world_name)_crazyflie.world" />
+    <arg name="debug" value="$(arg debug)" />
+    <arg name="paused" value="$(arg paused)"/>
+    <arg name="gui" value="$(arg gui)" />
+    <arg name="verbose" value="$(arg verbose)"/>
+  </include>
+
+  <group ns="$(arg mav_name)">
+  <!-- The following lines simulate the Crazyflie dynamics -->
+    <include file="$(find rotors_gazebo)/launch/spawn_mav_crazyflie.launch">
+      <arg name="mav_name" value="$(arg mav_name)" />
+      <arg name="model" value="$(find rotors_description)/urdf/mav_generic_odometry_sensor.gazebo" />
+      <arg name="enable_logging" value="$(arg enable_logging)" />
+      <arg name="enable_ground_truth" value="$(arg enable_ground_truth)" />
+      <arg name="enable_vi_sensor" value="$(arg enable_vi_sensor)"/>
+      <!-- As for the Mellinger and Position controllers in the "crazyflie2_hovering_example.launch" file -->
+      <arg name="enable_internal_model_controller" value="$(arg enable_internal_model_controller)" />
+      <arg name="log_file" value="$(arg log_file)"/>
+    </include>
+   <!-- The Crazyflie controller -->
+   <node name="position_controller_node" pkg="rotors_control" type="position_controller_node" output="screen">
+      <!-- Enabling internal model controller-->
+      <param name="enable_internal_model_controller" value="$(arg enable_internal_model_controller)"  />
+      <!-- Crazyflie file parameters used within the Internal Model Controller controller -->
+      <rosparam command="load" file="$(find rotors_gazebo)/resource/crazyflie_parameters.yaml" />
+      <rosparam command="load" file="$(find rotors_gazebo)/resource/$(arg mav_name).yaml" />
+      <!-- Loading Internal Model Controller's parameters -->
+      <rosparam command="load" file="$(find rotors_gazebo)/resource/crazyflie_internal_model_controller.yaml" />
+   </node>
+   <!-- Enable/Disable the trajectory generator - If the position_controller is activated, the hovering_example will be executed,
+        otherwise the spline generator and the Mellinger's controller will be run-->
+   <node name="hovering_example_spline" pkg="rotors_gazebo" type="hovering_example_spline" output="screen" >
+     <rosparam command="load" file="$(find rotors_gazebo)/resource/spline_trajectory.yaml" />
+   </node>
+   <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" />
+   <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" />
+   <node name="quaternion_to_rpy" pkg="rotors_gazebo" type="quaternion_to_rpy" output="screen" >
+       <remap from="odometry" to="odometry_sensor1/odometry" />
+   </node>
+
+  <!-- Visualisation RVIZ -->
+  <node name="$(anon rviz)" launch-prefix="$(arg launch_prefix)" pkg="rviz" type="rviz" respawn="false"
+	args="$(arg command_args)" output="screen" />
+
+  </group>
+</launch>
diff --git a/rotors_gazebo/launch/mav_hovering_example_with_vi_sensor.launch b/rotors_gazebo/launch/mav_hovering_example_with_vi_sensor.launch
index 312e164..44df143 100644
--- a/rotors_gazebo/launch/mav_hovering_example_with_vi_sensor.launch
+++ b/rotors_gazebo/launch/mav_hovering_example_with_vi_sensor.launch
@@ -1,3 +1,4 @@
+<?xml version="1.0"?>
 <launch>
   <arg name="mav_name" default="firefly"/>
   <arg name="world_name" default="basic"/>
diff --git a/rotors_gazebo/launch/spawn_mav_crazyflie.launch b/rotors_gazebo/launch/spawn_mav_crazyflie.launch
index 5451346..9034398 100644
--- a/rotors_gazebo/launch/spawn_mav_crazyflie.launch
+++ b/rotors_gazebo/launch/spawn_mav_crazyflie.launch
@@ -16,6 +16,7 @@
   <arg name="enable_mavlink_interface" default="false"/>
   <arg name="enable_mellinger_controller" default="false"/>
   <arg name="enable_internal_model_controller" default="false"/>
+  <arg name="enable_vi_sensor" default="false"/>
 
   <!-- send the robot XML to param server -->
   <param name="robot_description" command="
@@ -26,6 +27,7 @@
     enable_ground_truth:=$(arg enable_ground_truth)
     enable_state_estimator:=$(arg enable_state_estimator)
     enable_mavlink_interface:=$(arg enable_mavlink_interface)
+    enable_vi_sensor:=$(arg enable_vi_sensor)
     log_file:=$(arg log_file)
     wait_to_record_bag:=$(arg wait_to_record_bag)
     mav_name:=$(arg mav_name)
diff --git a/rotors_gazebo/rviz/crazyflie_vi_sensor.rviz b/rotors_gazebo/rviz/crazyflie_vi_sensor.rviz
new file mode 100644
index 0000000..6931f28
--- /dev/null
+++ b/rotors_gazebo/rviz/crazyflie_vi_sensor.rviz
@@ -0,0 +1,178 @@
+Panels:
+  - Class: rviz/Displays
+    Help Height: 78
+    Name: Displays
+    Property Tree Widget:
+      Expanded:
+        - /Global Options1
+        - /Status1
+        - /Pose1
+        - /PointCloud21/Status1
+      Splitter Ratio: 0.5
+    Tree Height: 567
+  - Class: rviz/Selection
+    Name: Selection
+  - Class: rviz/Tool Properties
+    Expanded:
+      - /2D Pose Estimate1
+      - /2D Nav Goal1
+      - /Publish Point1
+    Name: Tool Properties
+    Splitter Ratio: 0.588679016
+  - Class: rviz/Views
+    Expanded:
+      - /Current View1
+    Name: Views
+    Splitter Ratio: 0.5
+  - Class: rviz/Time
+    Experimental: false
+    Name: Time
+    SyncMode: 0
+    SyncSource: Image
+Toolbars:
+  toolButtonStyle: 2
+Visualization Manager:
+  Class: ""
+  Displays:
+    - Alpha: 0.5
+      Cell Size: 1
+      Class: rviz/Grid
+      Color: 160; 160; 164
+      Enabled: true
+      Line Style:
+        Line Width: 0.0299999993
+        Value: Lines
+      Name: Grid
+      Normal Cell Count: 0
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Plane: XY
+      Plane Cell Count: 10
+      Reference Frame: <Fixed Frame>
+      Value: true
+    - Alpha: 1
+      Axes Length: 1
+      Axes Radius: 0.100000001
+      Class: rviz/Pose
+      Color: 255; 25; 0
+      Enabled: true
+      Head Length: 0.300000012
+      Head Radius: 0.100000001
+      Name: Pose
+      Shaft Length: 1
+      Shaft Radius: 0.0500000007
+      Shape: Arrow
+      Topic: /move_base_simple/goal
+      Unreliable: false
+      Value: true
+    - Class: rviz/Image
+      Enabled: true
+      Image Topic: /crazyflie2/vi_sensor/camera_depth/camera/image_raw
+      Max Value: 1
+      Median window: 5
+      Min Value: 0
+      Name: Image
+      Normalize Range: true
+      Queue Size: 2
+      Transport Hint: raw
+      Unreliable: false
+      Value: true
+    - Alpha: 1
+      Autocompute Intensity Bounds: true
+      Autocompute Value Bounds:
+        Max Value: 10
+        Min Value: -10
+        Value: true
+      Axis: Z
+      Channel Name: intensity
+      Class: rviz/PointCloud2
+      Color: 255; 255; 255
+      Color Transformer: RGB8
+      Decay Time: 0
+      Enabled: true
+      Invert Rainbow: false
+      Max Color: 255; 255; 255
+      Max Intensity: 4096
+      Min Color: 0; 0; 0
+      Min Intensity: 0
+      Name: PointCloud2
+      Position Transformer: XYZ
+      Queue Size: 10
+      Selectable: true
+      Size (Pixels): 3
+      Size (m): 0.00999999978
+      Style: Flat Squares
+      Topic: /crazyflie2/vi_sensor/camera_depth/depth/points
+      Unreliable: false
+      Use Fixed Frame: true
+      Use rainbow: true
+      Value: true
+  Enabled: true
+  Global Options:
+    Background Color: 48; 48; 48
+    Default Light: true
+    Fixed Frame: world
+    Frame Rate: 30
+  Name: root
+  Tools:
+    - Class: rviz/Interact
+      Hide Inactive Objects: true
+    - Class: rviz/MoveCamera
+    - Class: rviz/Select
+    - Class: rviz/FocusCamera
+    - Class: rviz/Measure
+    - Class: rviz/SetInitialPose
+      Topic: /initialpose
+    - Class: rviz/SetGoal
+      Topic: /move_base_simple/goal
+    - Class: rviz/PublishPoint
+      Single click: true
+      Topic: /clicked_point
+  Value: true
+  Views:
+    Current:
+      Class: rviz/Orbit
+      Distance: 12.8602066
+      Enable Stereo Rendering:
+        Stereo Eye Separation: 0.0599999987
+        Stereo Focal Distance: 1
+        Swap Stereo Eyes: false
+        Value: false
+      Focal Point:
+        X: 0
+        Y: 0
+        Z: 0
+      Focal Shape Fixed Size: true
+      Focal Shape Size: 0.0500000007
+      Invert Z Axis: false
+      Name: Current View
+      Near Clip Distance: 0.00999999978
+      Pitch: 0.785398006
+      Target Frame: <Fixed Frame>
+      Value: Orbit (rviz)
+      Yaw: 0.785398006
+    Saved: ~
+Window Geometry:
+  Camera:
+    collapsed: false
+  Displays:
+    collapsed: false
+  Height: 1056
+  Hide Left Dock: false
+  Hide Right Dock: false
+  Image:
+    collapsed: false
+  QMainWindow State: 000000ff00000000fd00000004000000000000016a00000396fc020000000afb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006100fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000028000002c6000000d700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000c00430061006d0065007200610000000252000000ca0000001600000016fb0000000a0049006d00610067006501000002f4000000ca0000001600ffffff000000010000010f00000396fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000002800000396000000ad00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073f0000003efc0100000002fb0000000800540069006d006501000000000000073f0000030000fffffffb0000000800540069006d00650100000000000004500000000000000000000004ba0000039600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  Selection:
+    collapsed: false
+  Time:
+    collapsed: false
+  Tool Properties:
+    collapsed: false
+  Views:
+    collapsed: false
+  Width: 1855
+  X: 65
+  Y: 24