Optitrack for Non-GPS Navigation

This article explains how a OptiTrack motion capture system can be used as a short-range substitute for a GPS allowing position control modes like Loiter, Guided, RTL, and Auto indoors.

Note

You will need a recent version of ArduPilot on your copter (Copter-4.0 or above).

Required hardware

Please refer to OptiTrack build your own tool for all hardware required to setup a motion capture system.

Note

In order to track the both location and orientation of a drone, you need at least 4 markers.

Motion capture system setup

Please refer to OptiTrack quick start guides for hardware and software setup. When you set the ground plane, do not forget to mark the origin and the axis. The positive X will point to the “north” of our indoor flight environment (because we do not use compass, it do not need pointing to the real/magnetic north)

../_images/optitrack_mark_ground_plane.jpg

Mark motion capture system coordinate system using tape

Required softwares

Prepare the drone

Warning

It is highly recommended that use small drone for indoor flight and deploy cage in your test flight enviorment.

Tip

If you are looking for a small drone for indoor flight test, Skyviper V2450 GPS drone or its successor journey GPS drone is a good choice. It is very affordable and running ArduPilot out-of-box. You can easily flash it wit custom build ArduPilot. If you perfer custom build small drone, there is a very good discuss here. The RTF quadcopter frame used in another example video is available from sdmodel.

First, you need to place markers on the drone. It is very important to place markers so that they form a stereoscopic, asymmetrical shape. Please refer to OptiTrack rigid body marker placement for details.

../_images/optitrack_place_markers.jpg

Place markers on the drone

Then, put the drone in the ground plane and align it with X axis of motion capture system. The drone should heading to positive X direction.

../_images/optitrack_drone_align_x.jpg

Align the drone with X axis

Select all markers in Motive and create a rigid body from them. Please refer to OptiTrack creating rigid body for details

Configuration the drone

Send data to the drone

Start MAVProxy and connect to your copter. Inside MAVProxy load optitrack module with:

module load optitrack

You need to set tracking rigid body id to match your setting in Motive:

optitrack set obj_id RIGID_BODY_STREAMING_ID

If you set Motive data streaming local interface to other than loopback , it is required to configuare optitrack module with:

optitrack set server SERVER_IP_ADDRESS
optitrack set client CLIENT_IP_ADDRESS

Note

The coordinate system of both Motive and ArduPilot are right-handed. While Z axis of ArduPilot is pointing down, Y axis of Motive is pointing up.

After all parameters is set, start sending pose to ardupilot:

optitrack start

Ground testing

  • Connect the drone to MAVProxy

  • Start Motive and make sure data streaming is turned on.

  • load and start optitrack module.

  • If you see following message appearing (initial pos may vary), then the drone is receiving pose data from Optitrack system.

EKF3 IMU0 is using external nav data EKF3 IMU0 initial pos NED = 0.1,-0.2,0.0 (m) EKF3 IMU0 ext nav yaw alignment complete

Flight testing

Take off in AltHold mode and maintain a stable hover. Switch to Loiter but be ready to switch back to AltHold or Stabilize if the vehicle’s position or altitude becomes unstable.

Note

In order to take off in guided or auto mode, you need to use GPS_GLOBAL_ORIGIN to set the GPS location of motion capture system origin. It is not need to be accurate, any valid lat/lng is ok.