Ultrasonic Positioning: 'Mass-Spring' Tracking Algorithm

Classmates of mine built a 3D Ultrasonic position system for a class project. I joined the project the next semester and developed a better tracking algorithm.

Ultrasonic positioning involves using sound waves to determine an object's distance from a number of known locations, and then computing the location of the object. There are a number of algorithms for doing it; one of the more popular is known as the Least Squares method. The method uses annoying linear algebra, and can break if given bad input data.

While trying to understand Least Squares, I developed a positioning/tracking algorithm that was more robust and feels more intuitive. Instead of building and inverting matrices, I create virtual springs between my known locations and the object I want to track. Since springs naturally want to settle to a certain 'rest' length, I set that length to be the ultrasonic range measurement for the given sensor. The object's initial position starts as a guess, and the springs apply forces until it reaches its true location.

Since the object is modeled as a mass, it has inertia and can resist impulse spikes from the springs due to bad data.


The simulation was written in Python. Visuals by VPython.