分為三步：

轉換傳感器數據

將傳感器數據從小車坐標系轉換到世界坐標系

通過轉換好的傳感器數據求解避讓運動參數

首先打開這個文件

首先在

取得轉換矩陣

%% START CODE BLOCK %%

            R = [cos(theta) -sin(theta) x; sin(theta) cos(theta) y; 0 0 1];

            %% END CODE BLOCK %%

在

實現轉換傳感器數據、將傳感器數據從小車坐標系轉換到世界坐標系

            %% START CODE BLOCK %%

            % Apply the transformation to robot frame.

            ir_distances_rf = zeros(3,n_sensors);

            for i=1:n_sensors

                x_s = obj.sensor_placement(1,i);

                y_s = obj.sensor_placement(2,i);

                theta_s = obj.sensor_placement(3,i);

                R = obj.get_transformation_matrix(x_s,y_s,theta_s);

                ir_distances_rf(:,i) = R*[ir_distances(i); 0; 1];

            end

            % Apply the transformation to world frame.

            [x,y,theta] = state_estimate.unpack();

            R = obj.get_transformation_matrix(x,y,theta);

            ir_distances_wf = R*ir_distances_rf;

            ir_distances_wf = ir_distances_wf(1:2,:);

            %% END CODE BLOCK %%

然后在

實現通過轉換好的傳感器數據求解避讓運動參數

            %% START CODE BLOCK %%   

            % Compute the heading vector    

            n_sensors = length(robot.ir_array);

            sensor_gains = [1 1 0.5 1 1];

            u_i = (ir_distances_wf-repmat([x;y],1,n_sensors))*diag(sensor_gains);

            u_ao = sum(u_i,2);

            % Compute the heading and error for the PID controller

            theta_ao = atan2(u_ao(2),u_ao(1));

            e_k = theta_ao-theta;

            e_k = atan2(sin(e_k),cos(e_k));

            %% END CODE BLOCK %%

最后運行