solve-lighthouse

#!/usr/bin/python
# Compute the position of a Lighthouse given three
# sensor readings in a known configuration.

from sympy import *
from sympy import solve_poly_system
from math import pi

# The default sensor array is 22mm square
# This fits easily on a breadboard.

r01 = 22
r02 = N(22*sqrt(2))
r03 = 22
r12 = 22
r13 = N(22*sqrt(2))
r23 = 22


# The few vector math functions that we need
def cross(a, b):
	return [
		a[1]*b[2] - a[2]*b[1],
		a[2]*b[0] - a[0]*b[2],
		a[0]*b[1] - a[1]*b[0]
	]

def vecmul(a, k):
	return [
		a[0]*k,
		a[1]*k,
		a[2]*k
	]
def vecsub(a, b):
	return [
		a[0] - b[0],
		a[1] - b[1],
		a[2] - b[2]
	]

def dot(a, b):
	return a[0]*b[0] + a[1]*b[1] + a[2]*b[2]

def len(a):
	return sqrt(dot(a,a))

def unitv(a):
	mag = len(a)
	return [a[0]/mag, a[1]/mag, a[2]/mag]

def ray(a1,a2):
	#print "a1=", a1*180/pi
	#print "a2=", a2*180/pi

	# Normal to X plane
	plane1 = [+cos(a1), 0, -sin(a1)]
	# Normal to Y plane
	plane2 = [0, +cos(a2), +sin(a2)]

	# Cross the two planes to get the ray vector
	return unitv(cross(plane2,plane1))

def tick2angle(a):
	return pi * (a / 48.0 - 4000) / 8333


# Translate them into angles, compute each ray vector for each sensor
# and then compute the angles between them
def lighthouse_pos(samples,id):
	v0 = ray(tick2angle(samples[0][id*2 + 1]), tick2angle(samples[0][id*2 + 2]))
	v1 = ray(tick2angle(samples[1][id*2 + 1]), tick2angle(samples[1][id*2 + 2]))
	v2 = ray(tick2angle(samples[2][id*2 + 1]), tick2angle(samples[2][id*2 + 2]))
	v3 = ray(tick2angle(samples[3][id*2 + 1]), tick2angle(samples[3][id*2 + 2]))

	v01 = dot(v0,v1)
	v02 = dot(v0,v2)
	v03 = dot(v0,v3)
	v12 = dot(v1,v2)
	v13 = dot(v1,v3)
	v23 = dot(v2,v3)

	#print "v0=", v0
	#print "v1=", v1
	#print "v2=", v2
	#print "v3=", v3
	print "v01=", acos(v01) * 180/pi, " deg"
	print "v02=", acos(v02) * 180/pi, " deg"
	print "v03=", acos(v03) * 180/pi, " deg"
	print "v12=", acos(v12) * 180/pi, " deg"
	print "v13=", acos(v13) * 180/pi, " deg"
	print "v23=", acos(v23) * 180/pi, " deg"

	k0, k1, k2, k3 = symbols('k0, k1, k2, k3')

	sol = nsolve((
		k0**2 + k1**2 - 2*k0*k1*v01 - r01**2,
		k0**2 + k2**2 - 2*k0*k2*v02 - r02**2,
		k0**2 + k3**2 - 2*k0*k3*v03 - r03**2,
		k2**2 + k1**2 - 2*k2*k1*v12 - r12**2,
		k3**2 + k1**2 - 2*k3*k1*v13 - r13**2,
		k2**2 + k3**2 - 2*k2*k3*v23 - r23**2,
	),
		(k0, k1, k2, k3),
		(1000,1000,1000,1000),
		verify=False  # ignore tolerance of solution
	)

	#print sol

	p0 = vecmul(v0,sol[0])
	p1 = vecmul(v1,sol[1])
	p2 = vecmul(v2,sol[2])
	p3 = vecmul(v3,sol[3])

	print "p0=", p0
	print "p1=", p1
	print "p2=", p2
	print "p3=", p3

	# compute our own estimate of the error
	print "err01=", len(vecsub(p0,p1)) - r01, " mm"
	print "err02=", len(vecsub(p0,p2)) - r02, " mm"
	print "err03=", len(vecsub(p0,p3)) - r03, " mm"
	print "err12=", len(vecsub(p1,p2)) - r12, " mm"
	print "err13=", len(vecsub(p1,p3)) - r13, " mm"
	print "err23=", len(vecsub(p2,p3)) - r23, " mm"


#
# The four parameter sets as input are the raw tick measurements
# in 48 MHz system clock values.
#
samples = [
[
[0,175004,129808,190646,183361,-811,-1142,-678,0.17],
[1,175188,130361,190558,182852,-796,-1146,-666,0.18],
[2,176268,130299,189730,182896,-773,-1131,-686,0.18],
[3,176087,129761,189826,183408,-788,-1127,-698,0.17],
],
[

[0,201179,173827,140827,145540,812,-195,-345,1.43],
[1,200599,173941,141550,144782,811,-210,-325,1.44],
[2,200970,174091,140291,143880,834,-202,-325,1.46],
[3,201536,173983,139576,144641,835,-186,-345,1.45],

],
[
[0,181300,99643,186370,200775,-1197,-993,-1406,0.33],
[1,182807,100136,185753,200642,-1165,-975,-1412,0.33],
[2,183529,99257,185501,200950,-1167,-970,-1434,0.34],
[3,181996,98797,186086,201082,-1198,-987,-1428,0.34],

],
[
[0,174711,135825,191565,177212,-690,-1222,-497,0.29],
[1,173750,135879,192416,177095,-712,-1240,-475,0.29],
[2,174009,136363,192252,176523,-694,-1244,-465,0.30],
[3,174959,136321,191400,176646,-672,-1226,-487,0.30],

],
[
[0,174749,135697,191518,177368,-692,-1219,-502,0.29],
[1,173763,135705,192382,177304,-716,-1237,-481,0.29],
[2,173946,136191,192289,176737,-700,-1242,-469,0.30],
[3,174906,136190,191433,176797,-677,-1224,-490,0.30],

],
[
[0,250199,165941,107596,186685,1070,512,-1378,0.90],
[1,249499,165939,108255,186651,1057,506,-1369,0.90],
[2,249165,166217,107763,186003,1068,505,-1360,0.91],
[3,249835,166220,107102,186029,1081,511,-1368,0.92],
],




]

for sample in samples:
	print sample
	lighthouse_pos(sample, 0)
	lighthouse_pos(sample, 1)