任意离散曲线包络线

%%   数据边界
clear all; close all;clc;
% %%   数据
data = load ('ftp_75_gear.txt');            %导入工况数据
data=data(1:end,:);
time=data(:,1);
speed=data(:,2);

%% 求出曲线的垂直距离为2的包络线
 x=time;
 y=speed;
 r=2;
 ratio=zeros(length(x),1);
 %%  对于一个点有两个斜率，做两条垂线，按照垂线相交点
for i=1:1:length(x)-1  %求斜率
    deta_y=y(i+1)-y(i);
    deta_x=x(i+1)-x(i);
    ratio(i)=atan(deta_y/deta_x);
end
ratio(length(x))=ratio(length(x)-1);

ratio_m=zeros(length(x),1);  %求斜率
ratio_m(1)=0;
ratio_m(2:length(x))=ratio(1:length(x)-1);



%% 求出曲线的垂直距离为2的包络线
for i=1:length(x)
    k=pi/2+ratio(i);
    a0(i)=x(i)+r*cos(k);
    b0(i)=y(i)+r*sin(k);
end
%%   同一个点的第二个值
for i=1:length(x)
    k=pi/2+ratio_m(i);
    a_1(i)=x(i)+r*cos(k);
    b_1(i)=y(i)+r*sin(k);
end



for i=2:length(x)-1
    x_intersect=a0(i);
    y_intersect=b0(i);
    if( abs(ratio(i)-ratio_m(i))>5/180*pi)
        m_x=[a0(i),a_1(i+1)];
        m_y=[b0(i),b_1(i+1)];
        p1=polyfit(m_x,m_y,1);
        
        m_x1=[a0(i-1),a_1(i)];
        m_y1=[b0(i-1),b_1(i)];
        p2=polyfit(m_x1,m_y1,1);
        
        x_intersect = fzero(@(x) polyval(p1-p2,x),3);
        y_intersect = polyval(p1,x_intersect);
    end
    x_plot(i)=x_intersect;
    y_plot(i)=y_intersect;
end
    x_plot(length(x))=x_plot(length(x)-1);
    y_plot(length(x))= y_plot(length(x)-1);

%% 求每点竖直线与包络线的交点
a_m=zeros(length(x_plot),1);
for i=1:length(x)
    record=-ones(length(x),1)*2;
    for j=1:length(x)-2
        if(    (  x(i)+10^(-8)>min(x_plot(j),x_plot(j+1))|| x(i)==min(x_plot(j),x_plot(j+1))  )  ...
                &&  (x(i)<max(x_plot(j),x_plot(j+1))|| x(i)==max(x_plot(j),x_plot(j+1))   )  )
                m_x=x_plot(j:j+1);
                m_y=y_plot(j:j+1);
                fit_par=polyfit(m_x,m_y,1);% 计算一次函数的斜率 与 b
                record(j)= x(i)*fit_par(1)+fit_par(2);
        end  
    end
    a_m(i)=max(record);
end



%%    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%% 求出曲线的垂直距离为2的包络线
for i=1:length(x)
    k=pi/2+ratio(i);
    a0(i)=x(i)-r*cos(k);
    b0(i)=y(i)-r*sin(k);
end
%%   同一个点的第二个值
for i=1:length(x)
    k=pi/2+ratio_m(i);
    a_1(i)=x(i)-r*cos(k);
    b_1(i)=y(i)-r*sin(k);
end



for i=2:length(x)-1
    x_intersect=a0(i);
    y_intersect=b0(i);
    if( abs(ratio(i)-ratio_m(i))>5/180*pi)
        m_x=[a0(i),a_1(i+1)];
        m_y=[b0(i),b_1(i+1)];
        p1=polyfit(m_x,m_y,1);
        
        m_x1=[a0(i-1),a_1(i)];
        m_y1=[b0(i-1),b_1(i)];
        p2=polyfit(m_x1,m_y1,1);
        
        x_intersect = fzero(@(x) polyval(p1-p2,x),3);
        y_intersect = polyval(p1,x_intersect);
    end
    xx_plot(i)=x_intersect;
    yy_plot(i)=y_intersect;
end
    xx_plot(length(x))=xx_plot(length(x)-1);
    yy_plot(length(x))= yy_plot(length(x)-1);

%% 求每点竖直线与包络线的交点
a_mm=zeros(length(x),1);
for i=1:length(x)
    record=ones(length(x),1)*100;
    for j=1:length(x)-1
        if(    (  x(i)>min(xx_plot(j),xx_plot(j+1))|| x(i)==min(xx_plot(j),xx_plot(j+1))  )  ...
                &&  (x(i)<max(xx_plot(j),xx_plot(j+1))|| x(i)==max(xx_plot(j),xx_plot(j+1))   )  )
                m_x=xx_plot(j:j+1);
                m_y=yy_plot(j:j+1);
                fit_par=polyfit(m_x,m_y,1);% 计算一次函数的斜率 与 b
                record(j)= x(i)*fit_par(1)+fit_par(2);
        end  
    end
    a_mm(i)=min(record);
end

figure;
% plot(x,a_m-y,'k');
% hold on;
plot(x,y-a_mm,'k');
hold on;

figure;
plot(x,y,'r-');
hold on;
plot(x,a_m,'k');
hold on;
plot(x,a_mm,'k');
hold on;