Code for testing Davenport's (eigenvalued) and Markley's (SVD) solutions to Wahba's problem  1965
Wahba's problem was published in 1965, SIAM Review, Vol 7, No 3.
Wabha's problem in short is determining ones (the body's) attitude using a number or coregistered vectors in a reference frame and observation vectors in body coordinates.
Basically the problem is minimizing the following cost function to get R, the rotation matrix (or attitude quaternion):
L = 0.5 SUM a_i (b_i  R r_i)^2
where
a_i  is the weights (a in the code)
b_i  observations in body coordinates (rb in the code)
r_i  known database of coregistered datapoints in a reference coordinates (rr in the code)
the above is equivalent to solving in quaternion from:
L = lambda_0  trace(RB) = lambda_0  q' K q
where
q  is the attitude quaternion; and
K  is calculated as below
Please follow the code. You will see the Equations were simply implemented as in the article.
There is one change however, the author
preferred using Zipfel's order for the quaternion representation, thus:
q = [ q0 q1 q2 q3 ] = [ cos (the/2) e(1)sin(the/2) e(1)sin(the/2) e(1)sin(the/2) ]
Note that Body orientations can similarly be found with Gupta's 1998 method or
by simply coregistering a known stardatabase distance matrix based on observed
angulardistances. Note that Gupta's work seems to be based on Hyslop 1987 work.
The main source document used was "Humble Problems" by F. Landis Markley  2006. It was freely downloadable at the time of writing from: http://ntrs.nasa.gov/archive/nasa/casi.ntrs.nasa.gov/20060012294_2006013132.pdf
See also Julian Balfour's work in on threepoint tracking (basically a
distance matrix traversing method).
Note that to turn points the Tensor R is transpose(dcm) if you are using an allowable coordinate system. See the 1st or 2nd Edition of "Modeling and Simulation of Aerospace Vehicle Dynamics"  Zipfel.
1.4  Added the function rotatePoints which is part of the arrow3D (File ID: #12274) submission (slightly altered for addressing the problem). 

1.2  Files were not uploaded, with the bug fix. 

1.1  Bug fixed in the two dcm (tensor transposed) to quaternion conversions. 
Inspired by: 3D Arrow with many color/parameter options
AlexG (view profile)
Why are the diagonal elements of the K matrix switched from the version in "Humble Problems"?
Also, I found something I cannot understand. While using your files I defined my measurements in the Reference frame rr and then a rotation matrix R_zyx=R_x(phi)*R_y(theta)*R_z(psi), which is meant to be the rotation from Reference to Body frames. According to what I understand, it's supposed to take rr and apply R_zyx to derive rb=R_zyx*rr, but that yields wrong results. In fact, Using R_xyz=R_zyx', the rotation matrix from Body to Reference frame provides the right result (how?!). If I rotate using the quaternion method like you do, it works fine. But I'd like to understand what I'm missing about the rotation matrix in this case. Thank you.
Petri (view profile)
Dankie, goeie oplossing.