Armaturespace Coordinates

I’ve been banging my head against the wall with this for about the past week.

So I have a motion capture system which is giving me a stream of marker coordinates (x, y, z) at 60fps. I want to use these markers to move the armature of a character. The coordinates are absolute coordinates. I understand that bone head and tail coordinates are relative to the head/tail of their parent. So I specified a hierarchy for the markers, so that each marker has a parent. Then, to find a particular head or tail of a bone, I do bone_coordinate = parent_of_bone + parent_of_marker - marker_coordinate. That makes the armature move, but everything seems twisted. I’m thinking that I’m missing something about the coordinate system.

Help?

The source is here: http://paste.lisp.org/+1U4N