deadreckonsimpson.m

来自「该程序用PID系统实现差动转向车辆控制」· M 代码 · 共 59 行

%% Author: epokh
%% Website: www.epokh.org/drupy
%% This software is under GPL


function new_pos=deadReckonSimpson(ar,al,vr,vl,bw,old_position,t)
%% This function use the Simpson method to calculate the new points
%% step by step
           
           A = (al + ar) /2;
           B = (vl + vr) /2;
           C = (ar - al) /(2 * bw);
           D = (vr - vl) / bw;

           theta0 = old_position(3);

           theta = C*t*t + D*t + theta0;

           tstart = 0;
           tend = t;

           finalX = 0;
           finalY = 0;
           
%            //Start of Simpson's rule...
           simpsonIntervals = get_simpson_intervals(A, B, C, D, t);
           deltaT = t / simpsonIntervals;

%            //f(x0)
           finalX =finalX+ (A*tstart + B)*(cos(C*tstart*tstart + D*tstart + theta0));
           finalY =finalY+ (A*tstart + B)*(sin(C*tstart*tstart + D*tstart + theta0));

%            //4*f(x1) + 2*f(x2) + ... + 4*f(xn-1)
           for i =1:1:simpsonIntervals
                   tstart =tstart+ deltaT;
                   if (mod(i,2) == 1)
                           finalX =finalX+ 4 * (A*tstart + B) *(cos(C*tstart*tstart + D*tstart + theta0));
                           finalY =finalY+ 4 * (A*tstart + B) *(sin(C*tstart*tstart + D*tstart + theta0));
                   else
                           finalX =finalX+ 2 * (A*tstart + B) *(cos(C*tstart*tstart + D*tstart + theta0));
                           finalY =finalY+ 2 * (A*tstart + B) *(sin(C*tstart*tstart + D*tstart + theta0));
                   end
           end%%end for

%            //f(xn)
           finalX =finalX+ (A*tend + B) * (cos(C*tend^2 + D*tend + theta0));
           finalY =finalY+ (A*tend + B) * (sin(C*tend^2 + D*tend + theta0));

           finalX = deltaT*finalX;
           finalY = deltaT*finalY;

%            //End of simpson's rule...

        finalX=finalX+old_position(1);
        finalY=finalY+old_position(2);
        new_pos=[finalX,finalY,theta];   

end