求助python代码转火山

卢本伟

mport numpy as np
import 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[np.float64],
        prev_e: npt.NDArray[np.float64],
        prev_m: npt.NDArray[np.float64],
        dt: float,
    ):
        vel_raw = ((curr_e - prev_e) + prev_m) / dt

        for axis in range(2):
            if np.sign(vel_raw[axis]) != np.sign(curr_e[axis]):
                vel_raw[axis] = 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[axis]) != np.sign(curr_e[axis]):
                acc_raw[axis] = 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

这样的吗

a759077146

用卡尔曼预测