Iterative Solution of Weighted Least Squares Problems with Applications to Rigid Multibody Dynamics