请用matlab写一个在给定参考轨迹点序列情况下将含有位置姿态速度加速度的车辆信息从笛卡尔坐标转到frenet坐标系的代码其中同时输出s与d的一阶导数与二阶导数信息。不要使用插值函数

以下是使用matlab实现将车辆信息从笛卡尔坐标系转换到frenet坐标系的代码：

% 输入：ref_path：参考轨迹点序列，每个点包含x、y坐标信息
%       ego_state：车辆信息，包含位置、姿态、速度、加速度
% 输出：frenet_state：车辆信息在frenet坐标系下的表示，包含s、d坐标、一阶导数和二阶导数信息

% 初始化
frenet_state = struct();
frenet_state.s = zeros(size(ego_state, 1), 1);
frenet_state.d = zeros(size(ego_state, 1), 1);
frenet_state.ds = zeros(size(ego_state, 1), 1);
frenet_state.dd = zeros(size(ego_state, 1), 1);

% 计算每个车辆位置到参考轨迹点的距离
dists = zeros(size(ref_path, 1), 1);
for i = 1:size(ref_path, 1)
    dists(i) = norm(ref_path(i, :) - ego_state(1:2)');
end

% 找到距离最近的参考轨迹点，作为起点
[~, start_idx] = min(dists);

% 初始化s、d坐标
frenet_state.s(1) = 0;
frenet_state.d(1) = dot((ego_state(1:2)' - ref_path(start_idx, :)), ...
    [cos(ref_path(start_idx, 3))