Формула, которую вы написали для B, неверна. Из бумаги B * Rt_p равен тому, что у вас есть, но не самому B. Это означает, что фактическая формула для матрицы:
B=np.matmul(eq_b(qt_vector),transpose(Rt_p))/norm(Rt_p)^2
В основном вы добавляете этот дополнительный материал, чтобы при умножении на Rt_p
все термины, включающие Rt_p
отменены и у вас осталось eq_b(qt_vector)
. Отмена обусловлена тем, что
transpose(Rt_p)*Rt_p ==norm(Rt_p)^2