代码详解——如何计算航向误差？

N=30000;
T=0.05;
rr=zeros(N+10,4);
%参考轨迹
for ir=1:1:N
    if ir<=5100
        rr(ir,1)=19.5+0.1*(ir-1)*T;
        rr(ir,2)=80;
        rr(ir,3)=0;
        rr(ir,4)=0;
    elseif ir<=9787
        rr(ir,1)=45+15*sin(0.0067*(ir-5100)*T);
        rr(ir,2)=65+15*cos(0.0067*(ir-5100)*T);
        rr(ir,3)=0-0.0067*(ir-5100)*T;
        rr(ir,4)=-0.06666667;
    elseif  ir<=15787
        rr(ir,1)=60;
        rr(ir,2)=65-0.1*(ir-9787)*T;
        rr(ir,3)=-1.57;
        rr(ir,4)=0;
    elseif ir<=20473
        rr(ir,1)=75-15*cos(0.0067*(ir-15787)*T);
        rr(ir,2)=35-15*sin(0.0067*(ir-15787)*T);
        rr(ir,3)=-1.57+0.0067*(ir-15787)*T;
        rr(ir,4)=0.06666667;
    elseif ir<=30473
        rr(ir,1)=75+0.1*(ir-20473)*T;
        rr(ir,2)=20;
        rr(ir,3)=0;
        rr(ir,4)=0;
    else
        rr(ir,1)=150;
        rr(ir,2)=20;
        rr(ir,3)=0;
        rr(ir,4)=0;
    end
end
for i=1:1:15505
    xr0=x1(i,1);
    yr0=y1(i,1);
    xrdd=10000;
    xrdk=0;
    %计算参考路径上与车辆当前位置最近的点，并将该点在参考路径中的位置赋值给xrdk
    for ir=1:1:30000
        xrd(ir)=abs(sqrt((rr(ir,1)-xr0)^2+(rr(ir,2)-yr0)^2)); 
        if xrd(ir)<xrdd
            xrdd=xrd(ir);
            xrdk=ir;
            xr1=rr(ir,1);
            yr1=rr(ir,2);
        end
    end
    %计算参考路径上与车辆当前位置第二近的点
    if xrdk>1&&xrdk<30000
        xprd21=xrd(xrdk-1);
        xprd22=xrd(xrdk+1);
        if xprd21<xprd22
            xr2=rr(xrdk-1,1);
            yr2=rr(xrdk-1,2);
        else
            xr2=rr(xrdk+1,1);
            yr2=rr(xrdk+1,2);
        end
    elseif xrdk==1
        xr2=rr(xrdk+1,1);
        yr2=rr(xrdk+1,2);
    else
        xr2=rr(xrdk-1,1);
        yr2=rr(xrdk-1,2);
    end
    %计算位移误差
    an=(yr2-yr1);
    bn=(xr2-xr1);
    cn=bn*yr1-an*xr1;
    e(i,1)=abs(an*xr0-bn*yr0+cn)/sqrt(an^2+bn^2);
    %判断位移误差的正负，一般定义车辆在参考路径前进方向的左侧时误差为正
    if xrdk<30000
        Tmp=(rr(xrdk,2)-rr(xrdk+1,2))*xr0+(rr(xrdk+1,1)-rr(xrdk,1))*yr0+rr(xrdk,1)*rr(xrdk+1,2)-rr(xrdk+1,1)*rr(xrdk,2);
    else
        Tmp=(rr(xrdk-1,2)-rr(xrdk,2))*xr0+(rr(xrdk,1)-rr(xrdk-1,1))*yr0+rr(xrdk-1,1)*rr(xrdk,2)-rr(xrdk,1)*rr(xrdk-1,2);
    end
    if Tmp<=0
        e(i,1)=-e(i,1);
    end
    %计算航向
    rh=rr(xrdk,3);
    if i>1
        h=atan((yr0-y1(i-1,1))/(xr0-x1(i-1,1)));
    else
        h=atan((y1(i+1,1)-yr0)/(x1(i+1,1)-xr0));
    end
    if h>1%需根据实际行驶状态调整
        h=h-3.14;
    end
    %航向也可以用To Workspace模块直接读出，但是需要注意采样间隔应与x1、y1的To Workspace采样间隔一致。采用这种方法时不再需要专门根据实际情况对航向进行调整。
    %计算航向误差
    eh(i,1)=h-rh;
    %计算里程（用于绘图的横坐标，当然也可以绘制时序图，但是绘制时序图时，x1、y1的To Workspace采样间隔必须固定）
    ll(i,1)=xrdk*0.005;
    %tt(i,1)=i*0.05;%如果采样间隔为0.05s
end