diff --git a/ArduSub/Attitude.cpp b/ArduSub/Attitude.cpp
index 071e9fc5b8..c5dbc67e73 100644
--- a/ArduSub/Attitude.cpp
+++ b/ArduSub/Attitude.cpp
@@ -64,7 +64,7 @@ float Sub::get_roi_yaw()
     roi_yaw_counter++;
     if (roi_yaw_counter >= 4) {
         roi_yaw_counter = 0;
-        yaw_look_at_WP_bearing = get_bearing_cd(inertial_nav.get_position_xy(), roi_WP.xy());
+        yaw_look_at_WP_bearing = get_bearing_cd(inertial_nav.get_position_xy_cm(), roi_WP.xy());
     }
 
     return yaw_look_at_WP_bearing;
@@ -72,10 +72,10 @@ float Sub::get_roi_yaw()
 
 float Sub::get_look_ahead_yaw()
 {
-    const Vector3f& vel = inertial_nav.get_velocity();
-    const float speed = vel.xy().length();
+    const Vector3f& vel = inertial_nav.get_velocity_neu_cms();
+    const float speed_sq = vel.xy().length_squared();
     // Commanded Yaw to automatically look ahead.
-    if (position_ok() && (speed > YAW_LOOK_AHEAD_MIN_SPEED)) {
+    if (position_ok() && (speed_sq > (YAW_LOOK_AHEAD_MIN_SPEED * YAW_LOOK_AHEAD_MIN_SPEED))) {
         yaw_look_ahead_bearing = degrees(atan2f(vel.y,vel.x))*100.0f;
     }
     return yaw_look_ahead_bearing;
@@ -131,7 +131,7 @@ float Sub::get_surface_tracking_climb_rate(int16_t target_rate, float current_al
     static uint32_t last_call_ms = 0;
     float distance_error;
     float velocity_correction;
-    float current_alt = inertial_nav.get_altitude();
+    float current_alt = inertial_nav.get_position_z_up_cm();
 
     uint32_t now = AP_HAL::millis();
 
diff --git a/ArduSub/GCS_Mavlink.cpp b/ArduSub/GCS_Mavlink.cpp
index d2eb8057cb..5a91068b59 100644
--- a/ArduSub/GCS_Mavlink.cpp
+++ b/ArduSub/GCS_Mavlink.cpp
@@ -623,7 +623,7 @@ void GCS_MAVLINK_Sub::handleMessage(const mavlink_message_t &msg)
             if (packet.coordinate_frame == MAV_FRAME_LOCAL_OFFSET_NED ||
                     packet.coordinate_frame == MAV_FRAME_BODY_NED ||
                     packet.coordinate_frame == MAV_FRAME_BODY_OFFSET_NED) {
-                pos_vector += sub.inertial_nav.get_position();
+                pos_vector += sub.inertial_nav.get_position_neu_cm();
             } else {
                 // convert from alt-above-home to alt-above-ekf-origin
                 pos_vector.z = sub.pv_alt_above_origin(pos_vector.z);
diff --git a/ArduSub/Log.cpp b/ArduSub/Log.cpp
index 906e9f3225..a4ea5b29a5 100644
--- a/ArduSub/Log.cpp
+++ b/ArduSub/Log.cpp
@@ -39,7 +39,7 @@ void Sub::Log_Write_Control_Tuning()
         throttle_out        : motors.get_throttle(),
         throttle_hover      : motors.get_throttle_hover(),
         desired_alt         : pos_control.get_pos_target_z_cm() / 100.0f,
-        inav_alt            : inertial_nav.get_altitude() / 100.0f,
+        inav_alt            : inertial_nav.get_position_z_up_cm() * 0.01f,
         baro_alt            : barometer.get_altitude(),
         desired_rangefinder_alt   : (int16_t)target_rangefinder_alt,
         rangefinder_alt           : rangefinder_state.alt_cm,
diff --git a/ArduSub/commands_logic.cpp b/ArduSub/commands_logic.cpp
index 84fa971b09..25ced66be1 100644
--- a/ArduSub/commands_logic.cpp
+++ b/ArduSub/commands_logic.cpp
@@ -513,12 +513,12 @@ bool Sub::verify_circle(const AP_Mission::Mission_Command& cmd)
 
             // set target altitude if not provided
             if (is_zero(circle_center.z)) {
-                circle_center.z = inertial_nav.get_altitude();
+                circle_center.z = inertial_nav.get_position_z_up_cm();
             }
 
             // set lat/lon position if not provided
             if (cmd.content.location.lat == 0 && cmd.content.location.lng == 0) {
-                circle_center.xy() = inertial_nav.get_position_xy();
+                circle_center.xy() = inertial_nav.get_position_xy_cm();
             }
 
             // start circling
diff --git a/ArduSub/control_althold.cpp b/ArduSub/control_althold.cpp
index 43770f38a0..2e49e3c046 100644
--- a/ArduSub/control_althold.cpp
+++ b/ArduSub/control_althold.cpp
@@ -111,19 +111,19 @@ void Sub::control_depth() {
         // reset z targets to current values
         pos_control.relax_z_controller(channel_throttle->norm_input());
         engageStopZ = true;
-        lastVelocityZWasNegative = is_negative(inertial_nav.get_velocity_z());
+        lastVelocityZWasNegative = is_negative(inertial_nav.get_velocity_z_up_cms());
     } else { // hold z
 
         if (ap.at_bottom) {
             pos_control.init_z_controller();
-            pos_control.set_pos_target_z_cm(inertial_nav.get_altitude() + 10.0f); // set target to 10 cm above bottom
+            pos_control.set_pos_target_z_cm(inertial_nav.get_position_z_up_cm() + 10.0); // set target to 10 cm above bottom
         }
 
         // Detects a zero derivative
         // When detected, move the altitude set point to the actual position
         // This will avoid any problem related to joystick delays
         // or smaller input signals
-        if(engageStopZ && (lastVelocityZWasNegative ^ is_negative(inertial_nav.get_velocity_z()))) {
+        if(engageStopZ && (lastVelocityZWasNegative ^ is_negative(inertial_nav.get_velocity_z_up_cms()))) {
             engageStopZ = false;
             pos_control.init_z_controller();
         }
diff --git a/ArduSub/control_auto.cpp b/ArduSub/control_auto.cpp
index 422f7a974f..2494b24eb1 100644
--- a/ArduSub/control_auto.cpp
+++ b/ArduSub/control_auto.cpp
@@ -186,7 +186,7 @@ void Sub::auto_circle_movetoedge_start(const Location &circle_center, float radi
     // check our distance from edge of circle
     Vector3f circle_edge_neu;
     circle_nav.get_closest_point_on_circle(circle_edge_neu);
-    float dist_to_edge = (inertial_nav.get_position() - circle_edge_neu).length();
+    float dist_to_edge = (inertial_nav.get_position_neu_cm() - circle_edge_neu).length();
 
     // if more than 3m then fly to edge
     if (dist_to_edge > 300.0f) {
@@ -206,7 +206,7 @@ void Sub::auto_circle_movetoedge_start(const Location &circle_center, float radi
         }
 
         // if we are outside the circle, point at the edge, otherwise hold yaw
-        float dist_to_center = get_horizontal_distance_cm(inertial_nav.get_position_xy().topostype(), circle_nav.get_center().xy());
+        float dist_to_center = get_horizontal_distance_cm(inertial_nav.get_position_xy_cm().topostype(), circle_nav.get_center().xy());
         if (dist_to_center > circle_nav.get_radius() && dist_to_center > 500) {
             set_auto_yaw_mode(get_default_auto_yaw_mode(false));
         } else {
diff --git a/ArduSub/control_guided.cpp b/ArduSub/control_guided.cpp
index 7d8c2ae7e3..bcab8732d9 100644
--- a/ArduSub/control_guided.cpp
+++ b/ArduSub/control_guided.cpp
@@ -524,7 +524,7 @@ void Sub::guided_limit_init_time_and_pos()
     guided_limit.start_time = AP_HAL::millis();
 
     // initialise start position from current position
-    guided_limit.start_pos = inertial_nav.get_position();
+    guided_limit.start_pos = inertial_nav.get_position_neu_cm();
 }
 
 // guided_limit_check - returns true if guided mode has breached a limit
@@ -537,7 +537,7 @@ bool Sub::guided_limit_check()
     }
 
     // get current location
-    const Vector3f& curr_pos = inertial_nav.get_position();
+    const Vector3f& curr_pos = inertial_nav.get_position_neu_cm();
 
     // check if we have gone below min alt
     if (!is_zero(guided_limit.alt_min_cm) && (curr_pos.z < guided_limit.alt_min_cm)) {
diff --git a/ArduSub/inertia.cpp b/ArduSub/inertia.cpp
index b6182c6512..8fc70f85ca 100644
--- a/ArduSub/inertia.cpp
+++ b/ArduSub/inertia.cpp
@@ -17,9 +17,9 @@ void Sub::read_inertia()
         return;
     }
 
-    current_loc.alt = inertial_nav.get_altitude();
+    current_loc.alt = inertial_nav.get_position_z_up_cm();
 
     // get velocity, altitude is always absolute frame, referenced from
     // water's surface
-    climb_rate = inertial_nav.get_velocity_z();
+    climb_rate = inertial_nav.get_velocity_z_up_cms();
 }