AP_Math: add get_vel_correction_for_sensor_offset

libraries/AP_Math/AP_Math.cpp

@@ -338,6 +338,22 @@ bool rotation_equal(enum Rotation r1, enum Rotation r2)
     return (v1 - v2).length() < 0.001;
 }
 
+/*
+ * return a velocity correction (in m/s in NED) for a sensor's position given it's position offsets
+ * sensor_offset_bf is in meters in body frame (Foward, Right, Down)
+ * rot_ef_to_bf is a rotation matrix to rotate from earth-frame (NED) to body frame
+ * angular_rate is rad/sec
+ */
+Vector3f get_vel_correction_for_sensor_offset(const Vector3f &sensor_offset_bf, const Matrix3f &rot_ef_to_bf, const Vector3f &angular_rate)
+{
+    if (sensor_offset_bf.is_zero()) {
+        return Vector3f();
+    }
+
+    // correct velocity
+    const Vector3f vel_offset_body = angular_rate % sensor_offset_bf;
+    return rot_ef_to_bf.mul_transpose(vel_offset_body) * -1.0f;
+}
 
 #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
 // fill an array of float with NaN, used to invalidate memory in SITL