get_simpson_intervals.m

该程序用PID系统实现差动转向车辆控制

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


function N=get_simpson_intervals(A,B,C,D,x)
%%This function compute the number of simpson intervals during the
%%simulation

% Explanation:
% /*  To use Simpson's Rule for approximating the value of an
%       *  integral of f(x) over the interval [0, t], we need to
%       *  select enough sub-intervals so that the absolute error
%       *  is smaller than some arbitrary bound.   Simpson's Rule
%       *  provides a technique for estimating the upper limit of the
%       *  magnitude of the error based on the 4th derivative of f(x)
%       *
%       *     If |f4(x)| <= M for all x in interval a <= x <= b,
%       *     and h = (b-a)/n  then
%       *
%       *          Error <= M * h^4*(b-a)/180
%       *
%       *      where n is the number of subintervals.
%       *
%       *  For our position functions x(t)=(Ax+B)cos(Cx^2 +Dx +E), and
%       *  y(t)=(Ax+B)sin(Cx^2+Dx+E), the 4th derivatives get a little involved,
%       *  but fortunately we can take advantage of certain simplifications.
%       *  The 4th derivative of of the function f(x) = (Ax+B)cos(Cx^2 + Dx + E) is
%       *
%       *      f4(x) = 4Asin(Cx^2 + Dx + E)(2Cx + D)^3
%       *            - 24Acos(Cx^2 + Dx + E) (2Cx+ D)C
%       *            + (Ax+b)cos(Cx^2 + Dx + E) (2Cx + D) ^4
%       *            + 12(Ax+B)sin(Cx^2 + Dx + E) (2Cx+D)^2 * C
%       *            - 12(Ax+B)cos(Cx^2 + Dx + E)C^2
%       *
%       *  Because  |sin(x)| <= 1, |cos(x)|<=1, we know that the
%       *  maximum contribution of the absolute value of the trig functions,
%       *  no matter what the value of x, will be 1.  So we use that value,
%       *  replacing all trig functions with 1.   Now in the expression
%       *  (Ax+B)*sin(x), Ax+B may be less than zero.  But at the same
%       *  time sin(x) may also be negative, giving us a positive
%       *  value for (Ax+B)*sin(x).   Since we are interested in the
%       *  absolute, worst case, maximum value of the 4th derivative over
%       *  the interval, we take assume that each term makes a positive contribution
%       *  the f4(x) but taking absolute values.
%       *
%       *       M =  4|A(2Cx + D)^3| + 24|A(2Cx+ D)C|
%       *         +   |(Ax+b)(2Cx + D) ^4|  + 12|(Ax+B)(2Cx+D)^2 * C|
%       *         + 12|(Ax+B)C^2|
%       *
%       *  The same rule applies for g(x) = (Ax+B)sin(Cx^2 + Dx + E)
%       */
maxAllowableError = 0.01;      
term1   = A*x + B;
term2   = 2*C *x + D;
term2P2 = term2*term2;
term2P3 = term2P2 * term2;
term2P4 = term2P2 * term2P2;
xP5     = x^5;

Mcos =   abs(4*A * term2P3)+ abs(24 * A*C*term2)+ abs(term1 * term2P4)+abs(12 * term1 * term2P2 * C)+abs(12 * term1 * C *C );

Msin =   abs(4 * term2P3)+abs(24 * A * term2 * C)+abs(term1 * term2P4)+abs(12 * term1 * term2P2 * C)+abs(12 * term1 * C * C);

ncos = abs(Mcos * xP5 /(maxAllowableError*180));
nsin = abs(Msin * xP5 /(maxAllowableError*180));
n = ncos;

if (nsin > ncos)
    n = nsin;
end

n = n^ 0.25;

N =ceil(n);

if (mod(N,2)== 1)
    N =N+ 1;
end

if (N < 4)
    N = 4;
end

end