求助python代码转火山
mport numpy as npimport numpy.typing as npt
class Predictor:
ALPHA_VEL = 0.15
ALPHA_ACC = 0.15
def __init__(self):
self.smoothed_vel = np.zeros(2)
self.smoothed_acc = np.zeros(2)
self.prev_vel = np.zeros(2)
def predict(
self,
curr_e: npt.NDArray,
prev_e: npt.NDArray,
prev_m: npt.NDArray,
dt: float,
):
vel_raw = ((curr_e - prev_e) + prev_m) / dt
for axis in range(2):
if np.sign(vel_raw) != np.sign(curr_e):
vel_raw = 0.0
self.prev_vel = self.smoothed_vel.copy()
self.smoothed_vel = (
self.ALPHA_VEL * vel_raw + (1 - self.ALPHA_VEL) * self.smoothed_vel
)
acc_raw = (self.smoothed_vel - self.prev_vel) / dt
for axis in range(2):
if np.sign(acc_raw) != np.sign(curr_e):
acc_raw = 0.0
self.smoothed_acc = (
self.ALPHA_ACC * acc_raw + (1 - self.ALPHA_ACC) * self.smoothed_acc
)
pred_vel = self.smoothed_vel * dt
pred_acc = 0.5 * self.smoothed_acc * (dt**2)
return pred_vel + pred_acc
def reset(self):
self.smoothed_vel.fill(0)
self.smoothed_acc.fill(0)
self.prev_vel.fill(0)
用于物体轨迹追踪
这样的吗
用卡尔曼预测 huanhai 发表于 2025-12-18 18:21
这样的吗
应该是的:handshake
感谢无私分享
页:
[1]