Model AHRS after Sparton AHRS-M2

Signed-off-by: Michel Hidalgo <michel@ekumenlabs.com>

lrauv_description/models/tethys_equipped/model.sdf

@@ -76,29 +76,107 @@
             </noise>
           </ignition:current>
         </sensor>
+        <!--
+            Sparton AHRS-M2 arrangement of IMU + Magnetometer
+
+            See https://www.spartonnavex.com/product/ahrs-m2 for
+            AHRS specifications and additional documentation.
+        -->
         <sensor
             element_id="base_link"
             action="add"
-            name="ahrs_imu"
+            name="sparton_ahrs_m2_imu"
             type="imu">
+          <!--IMU in a Sparton AHRS-M2 -->
           <always_on>1</always_on>
-          <update_rate>10</update_rate>
+          <!-- Flip body frame to match FSK frame convention -->
           <pose degrees="true">0 0 0 180 0 0</pose>
           <orientation_reference_frame>
             <localization>NED</localization>
           </orientation_reference_frame>
           <enable_orientation>1</enable_orientation>
+          <update_rate>10</update_rate>
+          <linear_acceleration>
+            <!--
+                Use zero-mean gaussian distributions for each
+                linear acceleration component and its (turn on)
+                bias.
+
+                Standard deviation is assumed to be the same for all
+                linear acceleration components. It is computed from
+                reported noise density ND = 250 ug/√Hz and chosen
+                sample rate SR = 10 Hz as ND √SR.
+
+                Standard deviation is assumed to be the same for all
+                linear acceleration component (turn on) biases. It is
+                made equal to the reported bias stability BS = 0.07441 mg.
+            -->
+            <x>
+              <noise type="gaussian">
+                <stddev>0.00775283755402256</stddev>
+                <bias_stddev>0.007297128265</bias_stddev>
+              </noise>
+            </x>
+            <y>
+              <noise type="gaussian">
+                <stddev>0.00775283755402256</stddev>
+                <bias_stddev>0.007297128265</bias_stddev>
+              </noise>
+            </y>
+            <z>
+              <noise tzpe="gaussian">
+                <stddev>0.00775283755402256</stddev>
+                <bias_stddev>0.007297128265</bias_stddev>
+              </noise>
+            </z>
+          </linear_acceleration>
+          <angular_velocity>
+            <!--
+                Use zero-mean gaussian distributions for each
+                angular velocity component and its (turn on)
+                bias.
+
+                Standard deviation is assumed to be the same for all
+                angular velocity components. It is computed from
+                reported noise density ND = 0.04 °/s/√Hz and chosen
+                sample rate SR = 10 Hz as ND √SR.
+
+                Standard deviation is assumed to be the same for all
+                angular velocity component (turn on) biases. It is made
+                equal to the reported bias stability BS = 6.415 °/hr.
+            -->
+            <x>
+              <noise type="gaussian">
+                <stddev>0.0022076862812880228</stddev>
+                <bias_stddev>0.0017819444444444445</bias_stddev>
+              </noise>
+            </x>
+            <y>
+              <noise type="gaussian">
+                <stddev>0.0022076862812880228</stddev>
+                <bias_stddev>0.0017819444444444445</bias_stddev>
+              </noise>
+            </y>
+            <z>
+              <noise type="gaussian">
+                <stddev>0.0022076862812880228</stddev>
+                <bias_stddev>0.0017819444444444445</bias_stddev>
+              </noise>
+            </z>
+          </angular_velocity>
           <topic>/model/tethys/ahrs/imu</topic>
           <plugin filename="ignition-gazebo-imu-system"
                   name="ignition::gazebo::systems::Imu"/>
         </sensor>
+        <!--Magnetometer in a Sparton AHRS-M2 -->
         <sensor
             element_id="base_link"
             action="add"
             name="ahrs_magnetometer"
             type="magnetometer">
           <always_on>1</always_on>
           <update_rate>10</update_rate>
+          <!-- Flip body frame to match FSK frame convention -->
           <pose degrees="true">0 0 0 180 0 0</pose>
           <topic>/model/tethys/ahrs/magnetometer</topic>
           <plugin filename="ignition-gazebo-magnetometer-system"