diff --git a/models/gimbal_small_2d/model.sdf b/models/gimbal_small_2d/model.sdf
index 5d6b9921..5306e9ef 100644
--- a/models/gimbal_small_2d/model.sdf
+++ b/models/gimbal_small_2d/model.sdf
@@ -1,8 +1,8 @@
 <?xml version='1.0'?>
 <sdf version='1.9'>
   <model name='gimbal_small_2d'>
-    <pose>0 0 0.18 0 0 0</pose>
-    <link name='base_link'>
+    <!-- <pose>0 0 0.18 0 0 0</pose> -->
+    <link name='gimbal_link'>
       <inertial>
         <mass>0.2</mass>
         <inertia>
@@ -14,11 +14,11 @@
           <izz>0.0001</izz>
         </inertia>
       </inertial>
-      <visual name='base_main_viz'>
+      <visual name='gimbal_main_viz'>
         <geometry>
           <mesh>
             <scale>0.001 0.001 0.001</scale>
-            <uri>model://gimbal_small_2d/meshes/base_main.dae</uri>
+            <uri>package://ardupilot_gazebo/models/gimbal_small_2d/meshes/base_main.dae</uri>
           </mesh>
         </geometry>
         <material>
@@ -27,7 +27,7 @@
           <specular>0.01 0.01 0.01 1.0</specular>
         </material>
       </visual>
-      <collision name='base_col'>
+      <collision name='gimbal_col'>
         <pose>0.01 0.075 -0.025 0 0 0</pose>
         <geometry>
           <box>
@@ -52,7 +52,7 @@
         <geometry>
           <mesh>
             <scale>0.001 0.001 0.001</scale>
-            <uri>model://gimbal_small_2d/meshes/base_arm.dae</uri>
+            <uri>package://ardupilot_gazebo/models/gimbal_small_2d/meshes/base_arm.dae</uri>
           </mesh>
         </geometry>
         <material>
@@ -65,13 +65,13 @@
         <geometry>
           <mesh>
             <scale>0.001 0.001 0.001</scale>
-            <uri>model://gimbal_small_2d/meshes/base_arm.dae</uri>
+            <uri>package://ardupilot_gazebo/models/gimbal_small_2d/meshes/base_arm.dae</uri>
           </mesh>
         </geometry>
       </collision>
     </link>
     <joint name='roll_joint' type='revolute'>
-      <parent>base_link</parent>
+      <parent>gimbal_link</parent>
       <child>roll_link</child>
       <axis>
         <xyz>0 0 1</xyz>
@@ -102,7 +102,7 @@
         <geometry>
           <mesh>
             <scale>0.001 0.001 0.001</scale>
-            <uri>model://gimbal_small_2d/meshes/tilt.dae</uri>
+            <uri>package://ardupilot_gazebo/models/gimbal_small_2d/meshes/tilt.dae</uri>
           </mesh>
         </geometry>
         <material>
@@ -116,7 +116,7 @@
         <geometry>
           <mesh>
             <scale>0.001 0.001 0.001</scale>
-            <uri>model://gimbal_small_2d/meshes/tilt.dae</uri>
+            <uri>package://ardupilot_gazebo/models/gimbal_small_2d/meshes/tilt.dae</uri>
           </mesh>
         </geometry>
       </collision>
diff --git a/models/iris_with_gimbal/model.sdf b/models/iris_with_gimbal/model.sdf
index 960283dc..b4b97628 100755
--- a/models/iris_with_gimbal/model.sdf
+++ b/models/iris_with_gimbal/model.sdf
@@ -1,19 +1,20 @@
 <?xml version='1.0'?>
 <sdf version="1.9">
   <model name="iris_with_gimbal">
-    <include>
-      <uri>model://iris_with_standoffs</uri>
+    <include merge="true">
+      <uri>package://ardupilot_gazebo/models/iris_with_standoffs</uri>
+      <name>iris</name>
     </include>
 
-    <include>
-      <uri>model://gimbal_small_2d</uri>
+    <include merge="true">
+      <uri>package://ardupilot_gazebo/models/gimbal_small_2d</uri>
       <name>gimbal</name>
-      <pose>0 -0.01 0.070 1.57 0 1.57</pose>    
+      <pose>0 -0.01 -0.11 1.57 0 1.57</pose>
     </include>
 
     <joint name="gimbal_joint" type="revolute">
-      <parent>iris_with_standoffs::base_link</parent>
-      <child>gimbal::base_link</child>
+      <parent>base_link</parent>
+      <child>gimbal_link</child>
       <axis>
         <limit>
           <lower>0</lower>
@@ -27,6 +28,22 @@
     <plugin filename="gz-sim-joint-state-publisher-system"
       name="gz::sim::systems::JointStatePublisher">
     </plugin>
+    <plugin
+      filename="gz-sim-pose-publisher-system"
+      name="gz::sim::systems::PosePublisher">
+      <publish_link_pose>true</publish_link_pose>
+      <use_pose_vector_msg>true</use_pose_vector_msg>
+      <static_publisher>true</static_publisher>
+      <static_update_frequency>1</static_update_frequency>
+    </plugin>
+
+    <plugin
+      filename="gz-sim-odometry-publisher-system"
+      name="gz::sim::systems::OdometryPublisher">
+      <odom_frame>iris/odom</odom_frame>
+      <robot_base_frame>iris</robot_base_frame>
+      <dimensions>3</dimensions>
+    </plugin>
     <plugin filename="gz-sim-lift-drag-system"
         name="gz::sim::systems::LiftDrag">
       <a0>0.3</a0>
@@ -42,7 +59,7 @@
       <cp>0.084 0 0</cp>
       <forward>0 1 0</forward>
       <upward>0 0 1</upward>
-      <link_name>iris_with_standoffs::rotor_0</link_name>
+      <link_name>rotor_0</link_name>
     </plugin>
     <plugin filename="gz-sim-lift-drag-system"
         name="gz::sim::systems::LiftDrag">
@@ -59,7 +76,7 @@
       <cp>-0.084 0 0</cp>
       <forward>0 -1 0</forward>
       <upward>0 0 1</upward>
-      <link_name>iris_with_standoffs::rotor_0</link_name>
+      <link_name>rotor_0</link_name>
     </plugin>
 
     <plugin filename="gz-sim-lift-drag-system"
@@ -77,7 +94,7 @@
       <cp>0.084 0 0</cp>
       <forward>0 1 0</forward>
       <upward>0 0 1</upward>
-      <link_name>iris_with_standoffs::rotor_1</link_name>
+      <link_name>rotor_1</link_name>
     </plugin>
     <plugin filename="gz-sim-lift-drag-system"
         name="gz::sim::systems::LiftDrag">
@@ -94,7 +111,7 @@
       <cp>-0.084 0 0</cp>
       <forward>0 -1 0</forward>
       <upward>0 0 1</upward>
-      <link_name>iris_with_standoffs::rotor_1</link_name>
+      <link_name>rotor_1</link_name>
     </plugin>
 
     <plugin filename="gz-sim-lift-drag-system"
@@ -112,7 +129,7 @@
       <cp>0.084 0 0</cp>
       <forward>0 -1 0</forward>
       <upward>0 0 1</upward>
-      <link_name>iris_with_standoffs::rotor_2</link_name>
+      <link_name>rotor_2</link_name>
     </plugin>
     <plugin filename="gz-sim-lift-drag-system"
         name="gz::sim::systems::LiftDrag">
@@ -129,7 +146,7 @@
       <cp>-0.084 0 0</cp>
       <forward>0 1 0</forward>
       <upward>0 0 1</upward>
-      <link_name>iris_with_standoffs::rotor_2</link_name>
+      <link_name>rotor_2</link_name>
     </plugin>
 
     <plugin filename="gz-sim-lift-drag-system"
@@ -147,7 +164,7 @@
       <cp>0.084 0 0</cp>
       <forward>0 -1 0</forward>
       <upward>0 0 1</upward>
-      <link_name>iris_with_standoffs::rotor_3</link_name>
+      <link_name>rotor_3</link_name>
     </plugin>
     <plugin filename="gz-sim-lift-drag-system"
         name="gz::sim::systems::LiftDrag">
@@ -164,43 +181,67 @@
       <cp>-0.084 0 0</cp>
       <forward>0 1 0</forward>
       <upward>0 0 1</upward>
-      <link_name>iris_with_standoffs::rotor_3</link_name>
+      <link_name>rotor_3</link_name>
     </plugin>
 
     <plugin filename="gz-sim-apply-joint-force-system"
       name="gz::sim::systems::ApplyJointForce">
-      <joint_name>iris_with_standoffs::rotor_0_joint</joint_name>
+      <joint_name>rotor_0_joint</joint_name>
     </plugin>
     <plugin filename="gz-sim-apply-joint-force-system"
       name="gz::sim::systems::ApplyJointForce">
-      <joint_name>iris_with_standoffs::rotor_1_joint</joint_name>
+      <joint_name>rotor_1_joint</joint_name>
     </plugin>
     <plugin filename="gz-sim-apply-joint-force-system"
       name="gz::sim::systems::ApplyJointForce">
-      <joint_name>iris_with_standoffs::rotor_2_joint</joint_name>
+      <joint_name>rotor_2_joint</joint_name>
     </plugin>
     <plugin filename="gz-sim-apply-joint-force-system"
       name="gz::sim::systems::ApplyJointForce">
-      <joint_name>iris_with_standoffs::rotor_3_joint</joint_name>
+      <joint_name>rotor_3_joint</joint_name>
+    </plugin>
+
+    <plugin
+      filename="gz-sim-joint-position-controller-system"
+      name="gz::sim::systems::JointPositionController">
+      <joint_name>roll_joint</joint_name>
+      <topic>/gimbal/cmd_roll</topic>
+      <p_gain>2</p_gain>
+    </plugin>
+    <plugin
+      filename="gz-sim-joint-position-controller-system"
+      name="gz::sim::systems::JointPositionController">
+      <joint_name>tilt_joint</joint_name>
+      <topic>/gimbal/cmd_tilt</topic>
+      <p_gain>2</p_gain>
+    </plugin>
+
+    <plugin filename="gz-sim-linearbatteryplugin-system"
+      name="gz::sim::systems::LinearBatteryPlugin">
+      <battery_name>lipo_3500mAh</battery_name>
+      <fix_issue_225>true</fix_issue_225>
+      <open_circuit_voltage_constant_coef>12.6</open_circuit_voltage_constant_coef>
+      <open_circuit_voltage_linear_coef>-2.7</open_circuit_voltage_linear_coef>
+      <capacity>3.5</capacity>
+      <voltage>12.6</voltage>
+      <initial_charge>3.5</initial_charge>
+      <power_load>200.0</power_load>
+      <start_draining>false</start_draining>
     </plugin>
 
     <plugin name="ArduPilotPlugin"
       filename="ArduPilotPlugin">
-      <!-- Port settings -->
       <fdm_addr>127.0.0.1</fdm_addr>
       <fdm_port_in>9002</fdm_port_in>
       <connectionTimeoutMaxCount>5</connectionTimeoutMaxCount>
       <lock_step>1</lock_step>
-
-      <!-- Frame conventions
-        Require by ArduPilot: change model and gazebo from XYZ to XY-Z coordinates
-      -->
       <modelXYZToAirplaneXForwardZDown>0 0 0 3.141593 0 0</modelXYZToAirplaneXForwardZDown>
-      <gazeboXYZToNED>0 0 0 3.141593 0 0</gazeboXYZToNED>
+      <gazeboXYZToNED>0 0 0 3.141593 0 1.57079632</gazeboXYZToNED>
 
       <!-- Sensors -->
-      <imuName>iris_with_standoffs::imu_link::imu_sensor</imuName>
+      <imuName>imu_link::imu_sensor</imuName>
 
+      <!-- Control channels -->
       <!--
           incoming control command [0, 1]
           so offset it by 0 to get [0, 1]
@@ -209,7 +250,7 @@
           multiplier = 838 max rpm / 1 = 838
         -->
       <control channel="0">
-        <jointName>iris_with_standoffs::rotor_0_joint</jointName>
+        <jointName>rotor_0_joint</jointName>
         <useForce>1</useForce>
         <multiplier>838</multiplier>
         <offset>0</offset>
@@ -227,7 +268,7 @@
       </control>
 
       <control channel="1">
-        <jointName>iris_with_standoffs::rotor_1_joint</jointName>
+        <jointName>rotor_1_joint</jointName>
         <useForce>1</useForce>
         <multiplier>838</multiplier>
         <offset>0</offset>
@@ -245,7 +286,7 @@
       </control>
 
       <control channel="2">
-        <jointName>iris_with_standoffs::rotor_2_joint</jointName>
+        <jointName>rotor_2_joint</jointName>
         <useForce>1</useForce>
         <multiplier>-838</multiplier>
         <offset>0</offset>
@@ -263,7 +304,7 @@
       </control>
 
       <control channel="3">
-        <jointName>iris_with_standoffs::rotor_3_joint</jointName>
+        <jointName>rotor_3_joint</jointName>
         <useForce>1</useForce>
         <multiplier>-838</multiplier>
         <offset>0</offset>
@@ -281,7 +322,7 @@
       </control>
 
       <control channel="4">
-        <jointName>gimbal::roll_joint</jointName>
+        <jointName>roll_joint</jointName>
         <multiplier>3.14159265</multiplier>
         <offset>-0.5</offset>
         <servo_min>1100</servo_min>
@@ -292,7 +333,7 @@
       </control>
 
       <control channel="5">
-        <jointName>gimbal::tilt_joint</jointName>
+        <jointName>tilt_joint</jointName>
         <multiplier>3.14159265</multiplier>
         <offset>-0.5</offset>
         <servo_min>1100</servo_min>
@@ -304,20 +345,5 @@
 
     </plugin>
 
-    <plugin
-      filename="gz-sim-joint-position-controller-system"
-      name="gz::sim::systems::JointPositionController">
-      <joint_name>gimbal::roll_joint</joint_name>
-      <topic>/gimbal/cmd_roll</topic>
-      <p_gain>2</p_gain>
-    </plugin>
-    <plugin
-      filename="gz-sim-joint-position-controller-system"
-      name="gz::sim::systems::JointPositionController">
-      <joint_name>gimbal::tilt_joint</joint_name>
-      <topic>/gimbal/cmd_tilt</topic>
-      <p_gain>2</p_gain>
-    </plugin>
-
   </model>
 </sdf>
diff --git a/models/iris_with_standoffs/model.sdf b/models/iris_with_standoffs/model.sdf
index 16494e1b..4531a4c4 100755
--- a/models/iris_with_standoffs/model.sdf
+++ b/models/iris_with_standoffs/model.sdf
@@ -1,7 +1,7 @@
 <?xml version='1.0'?>
 <sdf version='1.9'>
   <model name='iris_with_standoffs'>
-    <pose>0 0 0.194923 0 0 0</pose>
+    <!-- <pose>0 0 0.194923 0 0 0</pose> -->
     <link name='base_link'>
       <velocity_decay>
         <linear>0.0</linear>
@@ -44,7 +44,7 @@
       <visual name='base_visual'>
         <geometry>
           <mesh>
-            <uri>model://iris_with_standoffs/meshes/iris.dae</uri>
+            <uri>package://ardupilot_gazebo/models/iris_with_standoffs/meshes/iris.dae</uri>
           </mesh>
         </geometry>
         <material>
@@ -129,6 +129,29 @@
           <specular>0.01 0.01 0.01 1.0</specular>
         </material>
       </visual>
+ 
+      <!-- Additional sensors -->
+      <sensor name="air_pressure_sensor" type="air_pressure">
+        <always_on>1</always_on>
+        <update_rate>30</update_rate>
+      </sensor>
+      <sensor name="air_speed_sensor" type="air_speed">
+        <always_on>1</always_on>
+        <update_rate>30</update_rate>
+      </sensor>
+      <sensor name="altimeter_sensor" type="altimeter">
+        <always_on>1</always_on>
+        <update_rate>30</update_rate>
+      </sensor>
+      <sensor name="magnetometer_sensor" type="magnetometer">
+        <always_on>1</always_on>
+        <update_rate>30</update_rate>
+      </sensor>
+      <sensor name="navsat_sensor" type="navsat">
+        <always_on>1</always_on>
+        <update_rate>30</update_rate>
+      </sensor>
+
     </link>
     <link name='imu_link'>
       <inertial>
@@ -201,7 +224,7 @@
         <geometry>
           <mesh>
             <scale>1 1 1</scale>
-            <uri>model://iris_with_standoffs/meshes/iris_prop_ccw.dae</uri>
+            <uri>package://ardupilot_gazebo/models/iris_with_standoffs/meshes/iris_prop_ccw.dae</uri>
           </mesh>
         </geometry>
         <material>
@@ -209,8 +232,8 @@
             <name>Gazebo/Blue</name>
             <uri>__default__</uri>
           </script-->
-          <ambient>0 0 1</ambient>
-          <diffuse>0 0 1</diffuse>
+          <ambient>0 0 1 1</ambient>
+          <diffuse>0 0 1 1</diffuse>
           <specular>0.1 0.1 0.1 1</specular>
         </material>
       </visual>
@@ -273,17 +296,17 @@
         <geometry>
           <mesh>
             <scale>1 1 1</scale>
-            <uri>model://iris_with_standoffs/meshes/iris_prop_ccw.dae</uri>
+            <uri>package://ardupilot_gazebo/models/iris_with_standoffs/meshes/iris_prop_ccw.dae</uri>
           </mesh>
         </geometry>
         <material>
           <!--script>
-            <name>Gazebo/White</name>
+            <name>Gazebo/Blue</name>
             <uri>__default__</uri>
           </script-->
-          <ambient>1 1 1 1</ambient>
-          <diffuse>1 1 1 1</diffuse>
-          <specular>.1 .1 .1 1</specular>
+          <ambient>0 0 1 1</ambient>
+          <diffuse>0 0 1 1</diffuse>
+          <specular>0.1 0.1 0.1 1</specular>
         </material>
       </visual>
       <gravity>1</gravity>
@@ -345,16 +368,16 @@
         <geometry>
           <mesh>
             <scale>1 1 1</scale>
-            <uri>model://iris_with_standoffs/meshes/iris_prop_cw.dae</uri>
+            <uri>package://ardupilot_gazebo/models/iris_with_standoffs/meshes/iris_prop_cw.dae</uri>
           </mesh>
         </geometry>
         <material>
           <!--script>
-            <name>Gazebo/Blue</name>
+            <name>Gazebo/Green</name>
             <uri>__default__</uri>
           </script-->
-          <ambient>0 0 1</ambient>
-          <diffuse>0 0 1</diffuse>
+          <ambient>0 1 0 1</ambient>
+          <diffuse>0 1 0 1</diffuse>
           <specular>0.1 0.1 0.1 1</specular>
         </material>
       </visual>
@@ -417,17 +440,17 @@
         <geometry>
           <mesh>
             <scale>1 1 1</scale>
-            <uri>model://iris_with_standoffs/meshes/iris_prop_cw.dae</uri>
+            <uri>package://ardupilot_gazebo/models/iris_with_standoffs/meshes/iris_prop_cw.dae</uri>
           </mesh>
         </geometry>
         <material>
           <!--script>
-            <name>Gazebo/White</name>
+            <name>Gazebo/Green</name>
             <uri>__default__</uri>
           </script-->
-          <ambient>1 1 1 1</ambient>
-          <diffuse>1 1 1 1</diffuse>
-          <specular>.1 .1 .1 1</specular>
+          <ambient>0 1 0 1</ambient>
+          <diffuse>0 1 0 1</diffuse>
+          <specular>0.1 0.1 0.1 1</specular>
         </material>
       </visual>
       <gravity>1</gravity>