Think i have cracked my location system

So after many pieces of paper I have the outline for my location system. I need to keep track of my head and body angle and then using these angles I can then calculate mine and an object I see location via triangulation. Again several conditionals will be needed to drive this system. Eg is the reference point on a sideline or an endline. is it too the left of centre or right of center etc. Gonna draw them up now and then write the sudo code and then implement the code. Wish me luck. Just need to decide whether its easier to locate the seen object using me as the refernece essentially the 0,0 then translating the vlaues into world co-ordinates or if I should just try get world co-ords from the object striaght away. Suppose a little more maths is called for :(