解决 Wahba 问题的代码：Code for testing Davenport's (eigen-valued) and Markley's (SVD) solutions to Wahba's problem - 1965-matlab开发

Wahba 的问题发表于 1965 年，SIAM 评论，第 7 卷，第 3 期。 简而言之，Wabha 的问题是使用参考系中的数字或共同注册的向量和身体坐标中的观察向量来确定（身体的）姿态。 基本上问题是最小化以下成本函数以获得 R，旋转矩阵（或姿态四元数）： L = 0.5 SUM a_i (b_i - R r_i)^2 在哪里a_i - 是权重（代码中的 a） b_i - 身体坐标中的观察值（代码中的 rb） r_i - 参考坐标中共同注册数据点的已知数据库（代码中的 rr） 以上相当于从以下四元数中求解： L = lambda_0 - 跟踪（RB）= lambda_0 - q' K q 在哪里q - 是姿态四元数； 和K - 计算如下 请按照代码。 您将看到方程式如文章中所述被简单地实现。 然而，有一个变化，作者首选使用 Zipfel 的四元数表示顺序，因此： q = [ q