Non-GPS Position Estimation

This page explains how MAVLink can be used to send in external position and velocity estimates to ArduPilot’s EKF allowing it to maintain a position estimate and thus control itself without a GPS

This is also called “External Navigation” although to be more precise it involves estimation rather than navigation or control.

Any of the following messages should be sent to the autopilot at 4hz or higher:

ArduPilot’s parameters should be setup as if a ModalAI VOXL is used which includes:

  • Set VISO_TYPE = 3 (VOXL)

  • Set VISO_POS_X, VISO_POS_Y, VISO_POS_Z to the camera’s position on the vehicle

  • Set EK3_SRC1_POSXY = 6 (ExternalNav)

  • Set EK3_SRC1_VELXY = 6 (ExternalNav) or 0 (None)

  • Set EK3_SRC1_POSZ = 6 (ExternalNav) or 1 (Baro)

  • Set EK3_SRC1_VELZ = 6 (ExternalNav) or 0 (None)

  • Set EK3_SRC1_YAW = 6 (ExternalNav) or 1 (Compass)

In addition it may be useful to setup GPS/Non-GPS transitions to allow switching between GPS and External Navigation

If no GPS is attached to the autopilot then the EKF origin must be set before the EKF can start estimating its position. If the vehicle will always be flown at the same location the ahrs-set-origin.lua script may be used

ODOMETRY message

The preferred method is to send an ODOMETRY with the fields populated as described below:

Field Name Type Description
time_usec uint64_t Timestamp since system boot. This does not need to be syncronised with the autopilot's time
frame_id uint8_t MAV_FRAME_BODY_FRD (12) or MAV_FRAME_LOCAL_FRD (20)
child_frame_id uint8_t MAV_FRAME_BODY_FRD (12) or MAV_FRAME_LOCAL_FRD (20)
x float X position in meters
y float Y position in meters
z float Z position in meters (positive is down)
q float[4] Quaternion components, w, x, y, z (1 0 0 0 is the null-rotation)
vx float X axis linear speed in m/s
vy float Y axis linear speed in m/s
vz float Z axis linear speed in m/s (positive is down)
rollspeed float Roll angular speed in rad/s (backwards is positive)
pitchspeed float Pitch angular speed in rad/s (forward is positive)
yawspeed float Yaw angular speed in rad/s (clockwise is positive)
pos_covariance float[21] elements 0, 6 and 11 are x, y and z-axis position error. 15, 18, 20 are roll, pitch and yaw angle error. Ignored if NaN
velocity_covariance float[21] not used
reset_counter uint8_t External estimator reset counter. This should be incremented when the estimate resets position, velocity, attitude or angular speed
estimator_type uint8_t not used
quality uint8_t quality metric as a percentage. -1 = odometry has failed, 0 = unknown/unset quality, 1 = worst quality, 100 = best quality

Six “pos_covariance” array elements are used to calculate the position error and angle error

  • 0: x-axis position error (m)

  • 6: y-axis position error (m)

  • 11: z-axis position error (m)

  • 15: roll angle error (rad)

  • 18: pitch angle error (rad)

  • 20: yaw angle error (rad)

For both position and angle error, the 3 elements are combined to create a single error which is used during EKF sensor fusion

../_images/nongps-position-estimation-poserr.png

In addition the position error is constrained to be between VISO_POS_M_NSE and 100 and the angle error between VISO_YAW_M_NSE and 1.5

Messages with “quality” below the VISO_QUAL_MIN parameter value are ignored

OPTICAL_FLOW message

An offboard optical flow sensor can send its horizontal flow sensor data to the autopilot using the OPTICAL_FLOW message

Users should set FLOW_TYPE = 5 (MAVLink) and then perform the Optical Flow Setup procedure before use

Field Name Type Description
time_usec uint64_t Timestamp since system boot. This does not need to be syncronised with the autopilot's time
sensor_id uint8_t not used
flow_x int16_t Flow rate around X-axis
flow_x int16_t Flow rate around Y-axis
flow_comp_m_x float not used
flow_comp_m_y float not used
quality uint8_t Optical flow quality 0:bad, 255:best
ground_distance float not used
flow_rate_x float not used
flow_rate_y float not used

Please see this issue 29131 for improvements we should make to the handling of this message

GPS/Non-GPS Transitions

The user wiki page for GPS/Non-GPS transitions is here

The EKF’s active source set can be changed in real-time by sending a COMMAND_LONG or COMMAND_INT message with the “command” field set to MAV_CMD_SET_EKF_SOURCE_SET (42007) and “param1” set to a number between 1 and 3