对上交代码进行修改，主要将能量机关去掉，添加了同济的PnP位姿解算，但是同济有个四元数，获取IMU部分没有启用，可能导致精度不够。当前还存在反陀螺功能，修改为逻辑和弹道预测相结合，主要在时间关系上进行调整。

tools/math_tools.hpp

@@ -0,0 +1,68 @@
+#pragma once
+// math_tools.hpp
+// 内联实现 solver.cpp 所需的全部数学工具函数，无外部依赖。
+
+#include <cmath>
+#include <Eigen/Dense>
+
+namespace tools
+{
+
+// ──────────────────────────────────────────────────────────────────────────────
+// square(x): 返回 x 的平方
+// ──────────────────────────────────────────────────────────────────────────────
+inline double square(double x) { return x * x; }
+
+// ──────────────────────────────────────────────────────────────────────────────
+// limit_rad(angle): 将弧度角限制在 (-π, π] 范围内
+// ──────────────────────────────────────────────────────────────────────────────
+inline double limit_rad(double angle)
+{
+  constexpr double two_pi = 2.0 * M_PI;
+  angle = std::fmod(angle + M_PI, two_pi);
+  if (angle < 0.0) angle += two_pi;
+  return angle - M_PI;
+}
+
+// ──────────────────────────────────────────────────────────────────────────────
+// eulers(R, ax0, ax1, ax2): 从旋转矩阵 R 提取欧拉角（内禀旋转，按 ax0->ax1->ax2 顺序）
+//
+// 常用调用方式：eulers(R, 2, 1, 0) 对应 ZYX（Yaw-Pitch-Roll）顺序。
+// 返回 Vector3d { angle_ax0, angle_ax1, angle_ax2 }
+// ──────────────────────────────────────────────────────────────────────────────
+inline Eigen::Vector3d eulers(const Eigen::Matrix3d & R, int ax0, int ax1, int ax2)
+{
+  // 使用 Eigen 内置的欧拉角接口（内禀旋转）
+  return R.eulerAngles(ax0, ax1, ax2);
+}
+
+// ──────────────────────────────────────────────────────────────────────────────
+// xyz2ypd(xyz): 将三维空间坐标转换为 [yaw, pitch, distance]
+//
+// 坐标系约定（与云台/世界坐标系一致）：
+//   x 轴：前方（射击方向）
+//   y 轴：左方
+//   z 轴：上方
+//
+// 返回 Vector3d { yaw, pitch, distance }（单位：弧度，弧度，米）
+// ──────────────────────────────────────────────────────────────────────────────
+inline Eigen::Vector3d xyz2ypd(const Eigen::Vector3d & xyz)
+{
+  double distance = xyz.norm();
+  double yaw      = std::atan2(xyz.y(), xyz.x());
+  double pitch    = std::atan2(xyz.z(), std::sqrt(xyz.x() * xyz.x() + xyz.y() * xyz.y()));
+  return Eigen::Vector3d(yaw, pitch, distance);
+}
+
+// ──────────────────────────────────────────────────────────────────────────────
+// get_abs_angle(a, b): 返回两个二维向量夹角的绝对值（弧度）
+// ──────────────────────────────────────────────────────────────────────────────
+inline double get_abs_angle(const Eigen::Vector2d & a, const Eigen::Vector2d & b)
+{
+  double cos_angle = a.normalized().dot(b.normalized());
+  // 数值钳位避免 acos 域错误
+  cos_angle = std::max(-1.0, std::min(1.0, cos_angle));
+  return std::acos(cos_angle);
+}
+
+}  // namespace tools