如何使用两个microbits（三轴磁力计）跟踪关节臂？

Question

我正在尝试使用两个微比特来跟踪人类手臂的运动。每个 microbit 都有一个 3d 轴磁力计和一个 3d 轴加速度计。我正在尝试仅使用磁力计来跟踪手臂的运动。

第一个 microbit 将定位在二头肌上，这些铜连接器指向肘部，z 轴指向二头肌。第二个 microbit 将像那样放置在前臂上

每个微位将从其磁力计产生一个指向其坐标系中北方的矢量。第一个微位的 -y 向量指向从肩膀到肘部的方向。第二个微位的 -y 向量指向从肘部到手的方向。 如果我的单位向量指向同一坐标系中两个微位的 -y 方向，我只需将每个向量乘以每个臂段的长度，在向量 1 的末尾绘制向量 2，瞧，一个完美的表示那个关节。

为此，我最初的计划是将第二个microbit生成的北向矢量投影到yz平面（出于所有意图和目的，两个臂段将始终包含在yz平面中），找到北向之间的夹角投影和向量 - y。使用这个角度，我会将第一个微位生成的北向量投影到 yz 平面上，然后旋转它通过这个角度，最终在微位 1 坐标系中找到微位 2 的 -y 向量。

import numpy as np
from kaspersmicrobit import KaspersMicrobit

def get_angle(v1, v2):
    v1_mag = np.linalg.norm(v1)
    v2_mag = np.linalg.norm(v2)
    v1_unit = v1 / v1_mag
    v2_unit = v2 / v2_mag
    cos_angle = np.dot(v1_unit, v2_unit)
    angle = np.arccos(cos_angle)
    cross_product = np.cross(v1_unit, v2_unit)
    if float(cross_product) < 0:
        angle = 2 * np.pi - angle
    return angle

class JointTracker:
    def __init__(self, *microbits: str | KaspersMicrobit) -> None:
        self.microbits = self.get_connection(microbits)
        self.vectors: List[np.ndarray(shape=3, dtype=float)] = []
        self.gravity_north_angle = None

    def update(self):
        vectors = [np.array([0, -1, 0])]
        north0 = self._get_magnetometer(self.microbits[0])
        north0_yz = np.array([north0[1], north0[2]])
        for microbit in self.microbits[1:]:
            northn = self._get_magnetometer(microbit)
            northn_yz = np.array([northn[1], northn[2]])
            theta = get_angle(np.array([-1, 0]), northn_yz)
            r = np.array([[1, 0, 0], [0, np.cos(theta), np.sin(theta)],
                         [0, -np.sin(theta), np.cos(theta)]])
            narm_vector = np.dot(
                r, np.array([0, north0[1], north0[2]]))\
                / np.linalg.norm(north0_yz)
            vectors.append(narm_vector)
        self.vectors = vectors
        # From here on down I'm also trying to track the "shoulder movement" so to speak. I try to
        # find the angles of the yz plane with respect to north by projecting the north vector 
        # onto the xy and xz planes and calculating the angle between these vectors and the -y
        # vector, then rotating all the vectors representing the arm by these angles.
        # But since I'm not even able to track the movement of the elbow, I decided to leave that 
        # for later.
        # rot_vectors = []
        # north0_xy = np.array([north0[0], north0[1]])
        # phi = get_angle(north0_xy, np.array([0, 1]))
        # rphi = np.array([[np.cos(phi), np.sin(phi), 0],
        #                 [-np.sin(phi), np.cos(phi), 0],
        #                 [0, 0, 1]])
        # north0_xz = np.array([north0[0], north0[2]])
        # gama = get_angle(north0_xz, np.array([0, 1]))
        # rgama = np.array([[np.cos(gama), 0, -np.sin(gama)],
        #                  [0, 1, 0],
        #                  [np.sin(gama), 0, np.cos(gama)]])
        # for vector in vectors:
        #    rot_vectors.append(np.dot(rgama, np.dot(rphi, vector)))
        # self.vectors = rot_vectors

我做了它并用 matplotlib 绘制了这些向量。而我看到的不是我想要的。

当将 microbits 定位为完全张开的手臂（手臂和前臂之间 180 度）时，前臂似乎相对于矢量表示中的手臂略微倾斜。当模拟关闭手臂时，矢量之间的角度似乎以实际角度的两倍速度减少。当真实矢量约为90度时，矢量表示中的角度约为100~110度。我注意到的另一件事是，根据我在哪里用微位进行测试，结果是不同的。我不确切地知道我做错了什么，老实说，我的想法和时间都用完了。我的代码有问题吗？我在几何部分做错了什么吗？我正在尝试做的事情有可能吗？ 请帮助。

完整的代码在 https://github.com/estevaopbs/microbit_tracker 但我相信我把所有需要的东西都放在了这篇文章中。