From: stach@fritz.sri.com (John Stach x6191) Subject: Algorithms for location/orientation Date: 23 Oct 91 16:35:44 GMT Organization: SRI International I am working on the problem of finding the position and orientation of a moving object through the remote measurement of time delays from transmitters. The transmitters and sensors may be located on the object or at a stationary position at known locations. The knowns are time delays and sensor/transmitter positions. The unknowns are object position and orientation. The problem is very similar to the problems of GPS navigation, and trackers for virtual-reality systems. Does anyone know of (1) the standard (or best) algorithms for the above problems, or (2) obvious approaches to a solution? Do people currently use Kalman filters or generalized nonlinear optimization? My current approach is a LS fit with constraints. Assume a moving object (known velocity) with several transmitters located on it and a stationary sensor. I can assume that the time delay measurement for each transmitter describes a spherical shell in which the sensor must lie. It is strightforward to write a set of linear equations in x,y, and z that is parametric in the distance to the center of the object. (x - x1)^2 + (y-y1)^2 + (z-z1)^2 = t1, (1) for the first transmitter, where x1,y1,z1 are referenced to a point on the object. For each measurement, x,y,z should be the same for the single sensor. Rewrite as; -2x1x - 2y1y - 2z1z = t1 - (x1^2 + y1^2 + z1^2) - (x^2 + y^2 + z^2) = t1 - d1 - D (2) where d1 is the distance from the reference point to the transmitter and D is the distance from the reference point to the sensor. Clearly D is a function of x,y, and z so this is not the end. I can conceptually vary D until my solution for x,y,z matches D. This translates into; 2 || D - (x^2 + y^2 + z^2) || min D which is straightforward to differentiate assuming a normal equations solution for x,y, and z. Note that since x,y, and z are in the object coordinate system, a known velocity of the object relative to its own coordinate system can be easily included in the solution. Comments? John Stach, (415) 859-6191, 301-65 SRI International 333 Ravenswood Ave Menlo Park, CA 94025