clc;
clear;
format long;
%绘制道路
% x = 0:180;
% y = ones(181,1)*[0 3 6];
% plot(x,y(:,1),'k',x,y(:,2),'--k',x,y(:,3),'k');
% axis([0 180 -2 10]);
% hold on;
%
%方法一：多项式插值拟合路径
x = [10 27 47 51 51 55 76 90]';
y = [10 30 30 35 59 65.5 65.5 90]';
[n,~] = size(x);
X = ones(n, n);
for i = 2 : n
X(:,i) = x.^(i-1);
end
A = X \ y;
x = 0 :160;
y = 0;
for i = 1 :n
y = y + A(i) * x.^(i-1);
end
h1 = plot(x,y,'-.');
hold on;
%方法二：Lagrange插值公式
X = [10 27 47 51 51 55 76 90]';
Y = [10 30 30 35 59 65.5 65.5 90]';
[n,~] = size(X);
syms x;
l = ones(n,1) + 0*x;
Px = 0*x;
for i = 1 : n
for j = 1 : n
if i == j
continue;
end
l(i) = l(i) * (x - X(j))/(X(i) - X(j));
end
Px = Px + l(i) * Y(i);
end
Px = matlabFunction(Px);
x = 0 : 160;
P = Px(x);
h2 = plot(x,P,'og');
hold on;
%方法三：Newton插值公式
x = [10 27 47 51 51 55 76 90]';
y = [10 30 30 35 59 65.5 65.5 90]';
[n,~] = size(x);
v_chashang = zeros(n - 1, n - 1);
for i = 1 : n - 1
v_chashang(i,1) = (y(i+1) - y(1))/(x(i+1) - x(1));
end
for j = 2 : n - 1
for i = j :n - 1
v_chashang(i,j) = (v_chashang(i,j-1) - v_chashang(j-1,j-1))/(x(i+1)-x(j));
end
end
X = 0 : 160;
Y = y(1);
for i = 1 : n - 1
lx = 1;
for j = 1 :i
lx = lx.*(X - x(j));
end
Y = Y + v_chashang(i,i).*lx;
end
h3 = plot(X,Y,'.m');
%方法四：Hermite
% x = [0 10 20 30 40 80 120 130 140 150 160]';
% y = [1.5 1.5 1.5 1.5 1.5 3 4.5 4.5 4.5 4.5 4.5]';
% dx = [0, 0];
% dy = [40, 120];
x = [10 27 47 51 51 55 76 90]';
y = [10 30 30 35 59 65.5 65.5 90]';
plot(x,y,'*b');
legend([h1, h2, h3],"多项式插值","Lagrange插值", "Newton插值");
