基于车辆动力学模型预测控制（任意轨迹）

Question

北理那本书第五章的基于动力学模型的s-function如何做到从外部输入任意的轨迹？
我见很多人都是偏差作为输入，以北理的代码为基础要如何改？

Answer 1

针对你的问题结合chatgpt知识库请参考以下内容：
北理那本书中第五章的基于动力学模型的S-Function实现中，是通过控制输入信号来控制系统状态的。可以在S-function中输入任意的轨迹，而不是只能输入偏差作为输入信号。

如果想要在北理的代码中进行修改，可以参考以下步骤：

1. 修改sfun_controller函数，在函数中根据需要输入的轨迹生成相应的输入信号。这里需要根据具体需求进行编写，例如可以通过解析CSV文件来读取轨迹数据，或者通过手动输入方式来指定轨迹。

2. 在Simulink中调用修改后的S-function，在调用时将输入信号指定为sfun_controller函数生成的信号。

需要注意的是，修改后的S-function需要进行充分的测试和验证，保证其能够正常地输入任意轨迹，并准确地控制系统状态。
修改代码的具体实现会因不同的需求而有所不同，因此以下仅提供修改的基本思路和方法。

在sfun_controller函数中，可以添加指定读取轨迹数据的代码：

/* Sample time handling */
#define SAMPLING_PERIOD_IDX 0
#define COMMAND_DATA_IDX 1
#define UNOPTIMIZED_LIMS_IDX 3
#define TRAJECTORY_DATA_IDX 4

void sfun_controller(SimStruct *S)
{
    /* get sample time */
    real_T sample_period = *mxGetPr(ssGetSFcnParam(S, SAMPLING_PERIOD_IDX));
    
    /* create input signal based on specified trajectory */
    mxArray *trajectory_data_mx = ssGetSFcnParam(S, TRAJECTORY_DATA_IDX);
    double *trajectory_data = mxGetPr(trajectory_data_mx);
    int n_trajectory_points = mxGetN(trajectory_data_mx);
    real_T *u = ssGetInputPortRealSignal(S, COMMAND_DATA_IDX);
    
    /* loop over simulation time steps */
    if (ssIsContinuousTask(S,0)) {
        /* get current time */
        time_now = ssGetT(S);
        
        /* generate input signal based on trajectory */
        int current_trajectory_point = (int) (time_now / sample_period);
        if(current_trajectory_point < n_trajectory_points) {
            u[0] = trajectory_data[current_trajectory_point];
        }
    }
}

这里，我们利用mxGetPr函数获取输入参数TRAJECTORY_DATA_IDX中存储的轨迹数据，并将轨迹点数存储到n_trajectory_points变量中。然后，通过判断当前时间time_now所处的轨迹点位置，将指定的轨迹值设置为输入信号u[0]。

在Simulink中调用修改后的S-Function时，需要将输入参数COMMAND_DATA_IDX的信号源指向该S-Function的输出端口。值得注意的是，轨迹数据需要按照指定格式填写，并通过S-Function的输入参数进行传递。

以上代码仅为示例，实际操作中可能会因具体应用场景而有所不同。因此，在进行代码修改之前，请务必对具体应用进行充分了解和测试，以确保代码能够正常工作。