High precision indoor positioning system for copter with 2cm accuracy

I’m a young researcher who is developing a precise indoor positioning & guidance system for mobile robot.
I want to share our recent work about a custom flight mode that holds copter position at 10cm accuracy using ONLY our positioning system and PID controller, no IMU fusion or anything else.

I believe anyone who is looking for an indoor positioning solution will enjoy this! I would love to hear feedback from the experts of this field! Make sure to check out the video description for more info. Any comment is appreciated. Enjoy!

Video: https://youtu.be/bBFClxwxh0M
Research Article: http://www.mdpi.com/2224-2708/6/3/12


1. About our developing SNSH Indoor Positioning System
This system aims to provide a solution for indoor positioning problem where GPS cannot reach or when accuracy is crucial, which focuses on low cost, high accuracy at high speed, scalable sensor network, and simple to implement as it doesn’t require PC to operate.

We don’t try to solve SLAM (Simultaneous localization and mapping) problems. Instead, this sensor network based positioning system is designed to support autonomous robot guidance task inside the building or fixed infrastructure. As this system provides absolute 3D position, it won’t suffer from position drift over time like other odometry techniques. It can be used to guide cargo truck precisely on the warehouse, or control quadcopter to do autonomous surveillance tasks inside a large office.

  • Technology: Ultrasonic and RF signal based on TDoA, sine wave detector, and nonlinear least square trilateration.
  • Accuracy: 2cm/axis.
  • Update rate: 40Hz, with interpolation can reach 100Hz.
  • Sensor network: managed by CAN bus, easy to extend, and resilient to signal noise.

For more academic details, please check our research article.

Receiver Node Prototype

Quadcopter model: Beaglebone Green & SNSH_PilotCape
2. About implementation of Position Hold Demonstration

In this demonstration, we use SNSH positioning system as the only source for 3D position information because we want to demonstrate its speed, accuracy, and stability when tracking quadcopter position. To implement the idea, we use Beaglebone Green and run a modified Ardupilot Flight controller, inspired by BBBMINI project. We designed a custom Pilot cape for Beaglebone, which includes IMU, barometer for flight controller, and also our ultrasonic and RF transmitter circuit.

PilotCape for BBG

The sensor network setup is similar to the one mentioned on the research article. It includes 5 sensor nodes mounted in 2m square frame, one coordinator for managing sensor network and send distance measurement results to Beaglebone.

5 receiver nodes mounted on frame of 2m x 2m x 2m

We want to design a positioning and guidance system for copter, where position is determined on board, along with Ardupilot flight controller as a compact system, no PC is required to minimize system delay. In the Ardupilot firmware, we have implemented nonlinear least square trilateration algorithm to determine copter position from the measured distance data sent from the coordinator. It runs on Usercode 100Hz fast loop. Kalman filter and interpolation are applied to increase position update speed to 100Hz, and reduce measurement noise. After that, the calculated position is fed to a PID controller to perform position hold.

As our positioning system isn’t supported by ArduCopter, we have created a new flight mode, which is based on Alt Hold, then we replace the target roll & pitch (which is from RC transmitter) with PID control output.

At the moment, we are developing trajectory control for quadcopter using the information for our positioning system combined with optical flow for higher resolution. As the project is under developing, we cannot share much. Source code support for Ardupilot should be opened when the project is completed.

If you are interested in funding this technology, or want to buy some of our prototype system, feel free to contact me. I will continue to update our work in trajectory control soon. Have a good day!


@nghiajenius very interesting paper :slight_smile:

Usage of a sine wave detection algorithm triggered by RF to address the drawbacks of the threshold detection method is a clever technique. Implementing a CAN network is a challenge by itself, it would be interesting to see some of it on github.

I have few questions:
Looking at table 1

Is the Portable Ultrasonic Indoor system referenced as 34 is equivalent to MarvelMind , or its a different system ?
Have you been able to address the 45 degree cone limitation?
How did you implemented this system to control a quadcopter as on showned on video.
Are you using RC Override or you are sending coordinates through MavLink ?

Thanks and congratulation for this work.

Fantastic, congrats.
Great in addition to witness this successful implementation on a Beaglebone Green.

Can you share your configurations on the advanced parameters window?
Do you have GPS_TYPE disabled?
What about BCN_TYPE?
What Firmware version do you use?
Can you expand this accuracy to guided mode and create custom scripts for it?

Im tying to do the same thing, but using Marvelmind as the positioning system.

Please see my post in case you encountered any of my issues


Any help is appreciated.

Very sorry for my late reply, I have been busy with my thesis project. Thank you so much for your interest in my work, I’m really appreciated. Answer to your questions:

  • The Portable Ultrasonic Indoor system is not MarvelMind, it is a different research system that we came across when writing this paper. As MarvelMind is not open its technology, we cannot put it in our comparison.
  • About the the 45 degree limitation: The ultrasonic sensor we used in our lab only have 35 degree effective transmit/receive angle, and the signal strength reduces significantly went extending the transmitting angle. Using the proposed sine wave detector and threshold method, I am only able to extend the angle to 45 degree, more than that will result in completely signal loss at the receiver. I have seen other ultrasonic transmitter with wider angle, but haven’t have a chance to try them yet.
  • About implementation: I have created a new flight mode based on Alt Hold, and replaced the target roll/pitch (from RC transmitter) by my PID output value. The nonlinear least square trilateration for position estimation also runs on Usercode of Arducopter, along with kalman filter and interpolation to increase position information to 100Hz and reduce measurement noise, then the PID controller use the position data to control the copter. Compass heading is used for coverting body frame to world frame.

I have also updated system description on my post, you can check them again for more info. If you think the system can be improved by any way, just tell me what you think. Thank you!

Thank you for your encouragement, and helped me make the post look better. This is my first post, so it is quite clumsy. I have also updated system description on my post, you can read them for more details of my work. I will update more info when it’s done!

This is my own designed positioning system, and it is not supported by ArduCopter firmware. I haven’t used MarvelMind system yet, so I don’t have much experience with it. I did read some posts about set up the indoor position system, and they mentioned that you must set GUIDE_NOGPS to use them. Some helpful links for you: marvelmind, indoor loiter

I think that you shouldn’t start with guide mode at beginning. You should try to config your positioning system, then try using them with Alt Hold first, then Loiter so that you can ensure the performance of your positioning system, and get ready to switch to stabilize in case of failure.

In case you interested in using our developing system, you can contact me too :rofl: