if (model_ == "CTRA") {
double px_pre, py_pre, velo_pre, yaw_pre, yawd_pre, a_pre;
if (fabs(yawd_k) > 0.001) {
px_pre = px_k + ((velo_k * yawd_k + a_k * yawd_k * deltat) * std::sin(yaw_k + yawd_k * deltat) +
a_k * std::cos(yaw_k + yawd_k * deltat) - velo_k * yawd_k * std::sin(yaw_k) -
a_k * std::cos(yaw_k)) / (yawd_k * yawd_k);
py_pre = py_k + ((-velo_k * yawd_k - a_k * yawd_k * deltat) * std::cos(yaw_k + yawd_k * deltat) +
a_k * std::sin(yaw_k + yawd_k * deltat) + velo_k * yawd_k * std::cos(yaw_k) -
a_k * std::sin(yaw_k)) / (yawd_k * yawd_k);
}
公式推导参考：https://zhuanlan.zhihu.com/p/389589611
