So I have a quad with a black-box estimator on it. The black box estimates the pose of the quad. I also have a Vicon system that I'm using to get the ground truth pose of the quad. I'm trying to transform the output from the black-box system into the coordinate frame of the Vicon system so I can compare the two.
I have two series' of points recorded using the whole setup that I am trying to use to compute this transformation (from one world frame to the other world frame).
If you're not familiar, it is possible to compute a transformation between two frames given a set of points in each frame.
I have implemented the method described in the paper Least-Squares Rigid Motion Using SVD
If it's not clear, if the transformation were working correctly, the points labeled 'transformed' displayed on the graph below would roughly overlap with those labeled 'Vicon'. As you can see, they only overlap for less than half a second.
Any suggestions? Ideas about what could be wrong?