请把下面的matlab代码转换成c+代码 MPC动力学算法hyh20230221clcclearclose all 参考轨迹设置vx = 25;dt = 001;xr = 0 25 001 200; r1 = 0096 xr - 60 - 12;r2 = 0096 xr - 120 - 12;dm1 = 25; dm2 = 25;dn1 = 36; dn2 = 36;phir = ata

#include #include #include using namespace std;

//表示状态量 void Get_Target_Point(double *xr, double *yr, double *phir, double *kappar, double *vehicle_state_now, double *err, int &idx_nearest, double &kr) { double x = vehicle_state_now[0]; double y = vehicle_state_now[1]; double phi = vehicle_state_now[2]; double vx = vehicle_state_now[3]; double vy = vehicle_state_now[4]; double phi_dot = vehicle_state_now[5]; int n = sizeof(xr) / sizeof(xr[0]); double d_min = pow((x - xr[0]), 2) + pow((y - yr[0]), 2); int min = 1; for (int i = 0; i < n; i++) { double d = pow((x - xr[i]), 2) + pow((y - yr[i]), 2); if (d < d_min) { d_min = d; min = i + 1; } } idx_nearest = min; double tor[2] = {cos(phir[idx_nearest]), sin(phir[idx_nearest])}; double nor[2] = {-sin(phir[idx_nearest]), cos(phir[idx_nearest])}; double d_err[2] = {x - xr[idx_nearest - 1], y - yr[idx_nearest - 1]}; double ed = nor[0] * d_err[0] + nor[1] * d_err[1]; double es = tor[0] * d_err[0] + tor[1] * d_err[1]; double projection_point_phir = phir[idx_nearest - 1] + kappar[idx_nearest - 1] * es; double ed_dot = vy * cos(phi - projection_point_phir) + vx * sin(phi - projection_point_phir); double ephi = phi - projection_point_phir; while (ephi > M_PI) ephi -= 2 * M_PI; while (ephi < -M_PI) ephi += 2 * M_PI; double s_dot = vx * cos(phi - projection_point_phir) - vy * sin(phi - projection_point_phir); s_dot = s_dot / (1 - kappar[idx_nearest - 1] * ed); double ephi_dot = phi_dot - kappar[idx_nearest - 1] * s_dot; kr = kappar[idx_nearest - 1]; err[0] = ed; err[1] = ed_dot; err[2] = ephi; err[3] = ephi_dot; }

//计算控制量 void MPC_control(double *err, double Lf, double Lr, double Iz, double vx, double Cf, double Cr, double m, double dt, double *kappar, int idx_nearest, double &U) { int Nx = 4; int Nu = 1; int Np = 60; int Nc = 30; double Qq[Nx * Np][Nx * Np], Rr[Nu * Nc][Nu * Nc]; for (int i = 0; i < Nx * Np; i++) { for (int j = 0; j < Nx * Np; j++) { if (i == j) Qq[i][j] = 3e5; else Qq[i][j] = 0; } } for (int i = 0; i < Nu * Nc; i++) { for (int j = 0; j < Nu * Nc; j++) { if (i == j) Rr[i][j] = 5e5; else Rr[i][j] = 0; } } double row = 10;

//模型部分（原运动学模型）
double A0[4][4] = {{0, 1, 0, 0},
                   {0, (Cf + Cr) / (m * vx), -(Cf + Cr) / m, (Lf * Cf - Lr * Cr) / (m * vx)},
                   {0, 0, 0