无人驾驶车辆模型预测控制 -6.2.2

%《无人驾驶车辆模型预测控制》6.2.2 P138
%此处的xi,yi是相对于（x0,y0）的坐标，即车辆坐标系
t=-1:0.1:1;
S=1;
x0=0;
y0=0;
v=10;
[x,y]=meshgrid(t);
xi=x-x0;
yi=y-y0;
kesi=0.01;

Cost=(S*v)./(xi.^2+yi.^2+kesi);
surf(x,y,Cost);
xlabel('X/m');
ylabel('Y/m');
zlabel('Cost');
axis([-1 1 -1 1 0 1000]);