The code for solving Wahba's problem
Updated 24 Mar 2014
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 co-registered 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
a_i - is the weights (a in the code)
b_i - observations in body coordinates (rb in the code)
r_i - known database of co-registered 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
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 co-registering a known star-database distance matrix based on observed
angular-distances. 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 three-point 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.
Pieter Reyneke (2023). The code for solving Wahba's problem (https://www.mathworks.com/matlabcentral/fileexchange/40032-the-code-for-solving-wahba-s-problem), MATLAB Central File Exchange. Retrieved .
MATLAB Release Compatibility
Platform CompatibilityWindows macOS Linux
Inspired by: 3D Arrow with many color/parameter options
Community Treasure Hunt
Find the treasures in MATLAB Central and discover how the community can help you!Start Hunting!
Discover Live Editor
Create scripts with code, output, and formatted text in a single executable document.
Added the function rotatePoints which is part of the arrow3D (File ID: #12274) submission (slightly altered for addressing the problem).
Files were not uploaded, with the bug fix.
Bug fixed in the two dcm (tensor transposed) to quaternion conversions.