求等距偏置曲线

我用MATLAB软件实现等距偏置曲线，数据算法检查过了，都没有问题，但是就是出现不了我想要的。我想要让偏置曲线的对应点在基曲线对应点的切线的的垂线上。我附上我的程序，帮忙改改好吗？这是我程序实现的，点与点没有对应在法线上；我想要这样的。以下是我的程序，帮忙改改，谢谢。
x=[7.3 9.7 12.2 14.8 17.7 20.3 22.9 25.4];
y=[25.0 40.0 55.0 85.0 112.0 145.0 190.0 230.0];
p=polyfit(x,y,2);

根据拟合数据绘制基曲线
x1=linspace(7,30);
y1=polyval(p,x1);%根据拟合方程求值
plot(x,y,'ro',x1,y1,'k','linewidth',2.5)
xlabel('x轴'),ylabel('y轴表示C(x)'),title('图1 基曲线')
hold on

计算对应点的斜率、夹角、偏置点
q=polyder(p);%函数对多项式求导
k=polyval(q,x);
theta=atan(k);
d=4;
x0=x-d.*sin(theta);
y0=y+d.*cos(theta);

根据离散点拟合等距偏置曲线
n=polyfit(x0,y0,2);
x2=linspace(0,28);
y2=polyval(n,x2);%新的离散拟合曲线求对应值
plot(x0,y0,'^',x2,y2,'m','linewidth',2.2)
xlabel('x轴'),ylabel('y轴表示C1(x)'),title('等距偏置曲线')