算法-卡尔曼运动模型公式推导CTRA

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