www.pudn.com > globalpathplanning.rar > optimizerpoint15.asv, change:2012-03-13,size:2151b


%龙格库塔求解公式15 
function point=optimizerpoint15(point,i,flag)%这里返回值为point对么???i表示第i个点 flag表示x,y,z 
    pointi=point(i); 
     
    switch flag 
    
    case 0 
            k1=-formula15(0,pointi,flag,0);   %这里公式15计算方程的后半部分。 
            k1=k1-1.6*(2*point(i).x-point(i-1).x-point(i+1).x);%系数伊塔等于0.8,2*0.8=1.6,下同。 
            k2=-formula15(0+0.01/2,pointi,flag,0.01*k1/2);       %0.01为认为设置的时间步长h。 
            k2=k2-1.6*(2*(point(i).x+0.01*k1/2)-point(i-1).x-point(i+1).x); 
            k3=-formula15(0+0.01/2,pointi,flag,0.01*k2/2); 
            k3=k3-1.6*(2*(point(i).x+0.01*k2/2)-point(i-1).x-point(i+1).x); 
            k4=-formula15(0+0.01,pointi,flag,0.01*k3); 
            k4=k4-1.6*(2*(point(i).x+0.01*k3)-point(i-1).x-point(i+1).x); 
            point(i).x=point(i).x+0.01*(k1+2*k2+2*k3+k4)/6; 
            return;                      
     case 1 
            k1=-formula15(0,pointi,flag,0); 
            k1=k1-1.6*(2*point(i).y-point(i-1).y-point(i+1).y); 
            k2=-formula15(0+0.01/2,pointi,flag,0.01*k1/2); 
            k2=k2-1.6*(2*(point(i).y+0.01*k1/2)-point(i-1).y-point(i+1).y); 
            k3=-formula15(0+0.01/2,pointi,flag,0.01*k2/2); 
            k3=k3-1.6*(2*(point(i).y+0.01*k2/2)-point(i-1).y-point(i+1).y); 
            k4=-formula15(0+0.01,pointi,flag,0.01*k3); 
            k4=k4-1.6*(2*(point(i).y+0.01*k3)-point(i-1).y-point(i+1).y); 
            point(i).y=point(i).y+0.01*(k1+2*k2+2*k3+k4)/6; 
            return; 
      case 2 
            k1=-formula15(0,pointi,flag,0); 
            k1=k1-1.6*(2*point(i).z-point(i-1).z-point(i+1).z); 
            k2=-formula15(0+0.01/2,pointi,flag,0.01*k1/2); 
            k2=k2-1.6*(2*(point(i).z+0.01*k1/2)-point(i-1).z-point(i+1).z); 
            k3=-formula15(0+0.01/2,pointi,flag,0.01*k2/2); 
            k3=k3-1.6*(2*(point(i).z+0.01*k2/2)-point(i-1).z-point(i+1).z); 
            k4=-formula15(0+0.01,pointi,flag,0.01*k3); 
            k4=k4-1.6*(2*(point(i).z+0.01*k3)-point(i-1).z-point(i+1).z); 
            point(i).z=point(i).z+0.01*(k1+2*k2+2*k3+k4)/6; 
            return;              
    end