正北坐标系和车辆坐标系下的heading转换

正北坐标系由组合导航给出，以正北为0，顺时针为正。本车的heading与期望heading 之间的夹角，delta_heading 计算方法如下：

function  [delta_heading] = CalHeading(heading_desire, heading_current)
index_store = 0;
index_current = 0;
index_store = floor(heading_desire/90);
index_current = floor(heading_current /90);
delta_heading = 0;

index_store = floor(heading_desire/90);
index_current = floor(heading_current /90);
delta_heading = 0;
 if abs(index_store - index_current) == 0 
        delta_heading =  heading_desire - heading_current;
    elseif abs(index_store - index_current) == 1 
        delta_heading =  heading_desire - heading_current;
    elseif index_store ==0 && index_current ==3 
        delta_heading =  heading_desire - heading_current + 360;
    elseif index_store ==3 && index_current ==0 
        delta_heading =  heading_desire - heading_current - 360;
   return;
 end
    
    if  abs(index_store - index_current) == 2 
        if index_store ==0 && index_current ==2
               if abs(heading_desire - heading_current)> 180
                delta_heading = - (heading_desire + 360 - heading_current);
            else
                delta_heading = heading_current - heading_desire;
            end
            return;
        end
        
        if  index_store ==2 && index_current ==0
            if  abs(heading_desire- heading_current) > 180
                delta_heading = 360 + heading_current - heading_desire;
            else
                delta_heading = heading_current - heading_desire ;
            end
            return;
        end
   
        if  index_store ==1 && index_current ==3
            if abs(heading_desire- heading_current) > 180
                delta_heading = - (heading_desire + 360 - heading_current);
            else
                delta_heading = heading_current - heading_desire;
            end
            return;
        end
        
        if index_store ==3 && index_current ==1
             if  abs(heading_desire- heading_current) > 180
                delta_heading = heading_current + 360 - heading_desire;  
            else
                delta_heading = -(heading_desire - heading_current);
            end
            return;
        end  
    end
end