Apollo v5.5.0代码解读之Math--Integral

求积方式

$$I={\int }_a^{b}f(x)dx$$

如果知道被积公式的原函数$F(x)$​，利用牛顿-莱布尼兹公式有：
$${\int }_a^{b}f(x)dx=F(b)-F(a)$$
此外，还可以使用分割法，分割成无穷多个小区间：
$${\int }_a^{b}f(x)dx=\underset{x\to \infty }{\lim }\sum _{i=1}^n\frac{b-a}{n}f(a+\frac{b-a}{n}i)$$
除了以上方式，还可以使用数值积分。

integral.h和integral.cc

高斯勒让德积分公式(GaussLegendre)

理论

$${\int }_a^{b}f(x)dx\approx \sum _{k=0}^n{A}_{k}f(x_k)$$

高斯–勒让德求积公式是构造高精度差值积分的最好方法之一。他是通过让节点和积分系数待定让函数$f(x)$​​​​以此取$i=0,1,2...n$​​​​​次多项式使其尽可能多的能够精确成立来求出积分节点和积分系数。高斯积分的代数精度是$2n-1$​​​​，而且是最高的。通常的运用的是$(-1,1)$​​​​的积分节点和积分系数，其他积分域是通过变换公式$x=(b-a)\ast t/2+(b+a)/2$​​​​变换到-1到1之间积分，其中$t\in (-1,1)$。​
$$\begin{array}{rl}& f(x)x\in (a,b)\\ & x=(b-a)\ast t/2+(b+a)/2t\in (-1,1)\\ \iff & f((b-a)\ast t/2+(b+a)/2)t\in (-1,1)\\ \iff & g(t)t\in (-1,1)\end{array}$$

$${\int }_a^{b}f(x)dx=\frac{b-a}{2}{\int }_{-1}^{1}f((b-a)\ast t/2+(b+a)/2)dt=\frac{b-a}{2}{\int }_{-1}^{1}g(t)dt$$

最终使用的标准形式：
$$\begin{gathered} {\int }_{-1}^{1}f(x)dx\approx \sum _{k=0}^n{A}_{k}f(x_k) \\ e.g.n=4 \\ {\int }_{-1}^{1}f(x)dx\approx A_{0}f(x_0)+A_{1}f(x_1)+A_{2}f(x_2)+A_{3}f(x_3)+A_{4}f(x_4) \\ 有五组(n+1)系数（x_k,A_k） \end{gathered}$$
下表列出高斯–勒让德求积公式的节点和系数

n	$x_k$	$A_k$
0	$0.0$	$2.0$
1	$\pm 0.577350269189625764507$	$1.0$
2	$0.0$ $\pm 0.774596669241483377010$​	$0.888888888888888888877$​​ $0.555555555555555555562$
3	$\pm 0.339981043584856264792$ $\pm 0.861136311594052575248$	$0.652145154862546142644$​​​ $0.347854845137453857383$​​
4	$0.0$ $\pm 0.538469310105683091018$ $\pm 0.906179845938663992811$	$0.568888888888888888883$ $0.478628670499366468030$ $0.236926885056189087515$
5	$\pm 0.661209386466264513688$ $\pm 0.238619186083196908630$ $\pm 0.932469514203152027832$​	$0.360761573048138607569$ $0.467913934572691047389$ $0.171324492379170345043$
6	$0.0$ $\pm 0.405845151377397166917$ $\pm 0.741531185599394439864$ $\pm 0.949107912342758524541$	$0.417959183673469387749$ $0.381830050505118944961$ $0.279705391489276667890$ $0.129484966168869693274$
7	$\pm 0.183434642495649804936$​ $\pm 0.525532409916328985830$​ $\pm 0.796666477413626739567$​ $\pm 0.960289856497536231661$​​	$0.362683783378361982976$​​​ $0.313706645877887287338$​​​ $0.222381034453374470546$​​​ $0.101228536290376259154$​​​
8	$0.0$​ $\pm 0.836031107326635794313$​ $\pm 0.968160239507626089810$ $\pm 0.324253423403808929042$ $\pm 0.613371432700590397285$	$0.330239355001259763154$​​​​ $0.180648160694857404059$​​​​ $0.812743883615744119737$​​​​ $0.312347077040002840057$ $0.260610696402935462313$​​​​
9	$\pm 0.148874338981631210881$​​​​​ $\pm 0.433395394129247190794$​​​​​ $\pm 0.679409568299024406207$​​​​​ $\pm 0.865063366688984510759$​​​​​ $\pm 0.973906528517171720066$​​​​​	$0.295524224714752870187$​​​​ $0.269266719309996355105$​​​​ $0.219086362515982044000$​​​​ $0.149451349150580593150$​​​​ $0.666713443086881375920$​​​​

代码实现

/**
 * @brief Compute the integral of a target single-variable function
 *        from a lower bound to an upper bound, by 5-th Gauss-Legendre method
 * Given a target function and integral lower and upper bound,
 * compute the integral approximation using 5th order Gauss-Legendre
 * integration.
 * The target function must be a smooth function.
 * Example:
 * target function: auto func = [](const double x) {return x * x;};
 *                  double integral = gauss_legendre(func, -2, 3);
 * This gives you the approximated integral of function x^2 in bound [-2, 3]
 *
 * reference: https://en.wikipedia.org/wiki/Gaussian_quadrature
 *            http://www.mymathlib.com/quadrature/gauss_legendre.html
 *
 * @param func The target single-variable function
 * @param lower_bound The lower bound of the integral
 * @param upper_bound The upper bound of the integral
 * @return The integral result
 */
template <std::size_t N>
double IntegrateByGaussLegendre(const std::function<double(double)>& func,
                                const double lower_bound,
                                const double upper_bound) {
  // 根据输入阶数，获取对应系数，这里N对应上表的n+1
  // Apollo提供了N从2到10，也就是上表n从1到9
  auto p = GetGaussLegendrePoints<N>();

  std::array<double, N> x = p.first;
  std::array<double, N> w = p.second;
  // 复化系数
  const double t = (upper_bound - lower_bound) * 0.5;
  const double m = (upper_bound + lower_bound) * 0.5;

  double integral = 0.0;
  for (size_t i = 0; i < N; ++i) {
    // 逐一累加各项和
    integral += w[i] * func(t * x[i] + m);
  }
  // 注意还乘t
  return integral * t;
}

/**
 * @brief Get the points and weights for different ordered Gauss-Legendre
 *        integration. Currently support order 2 - 10. Other input order will
 *        trigger compiling error.
 */
template <std::size_t N>
std::pair<std::array<double, N>, std::array<double, N>>
GetGaussLegendrePoints();

template <>
inline std::pair<std::array<double, 2>, std::array<double, 2>>
GetGaussLegendrePoints<2>() {
  std::array<double, 2> x;
  x[0] = -5.77350269189625764507e-01;
  x[1] = 5.77350269189625764507e-01;

  std::array<double, 2> w;
  w[0] = 1.0;
  w[1] = 1.0;

  return std::make_pair(x, w);
}

template <>
inline std::pair<std::array<double, 3>, std::array<double, 3>>
GetGaussLegendrePoints<3>() {
  std::array<double, 3> x;
  x[0] = 0.00000000000000000000e+00;
  x[1] = 7.74596669241483377010e-01;
  x[2] = -7.74596669241483377010e-01;

  std::array<double, 3> w;
  w[0] = 8.88888888888888888877e-01;
  w[1] = 5.55555555555555555562e-01;
  w[2] = 5.55555555555555555562e-01;

  return std::make_pair(x, w);
}

template <>
inline std::pair<std::array<double, 4>, std::array<double, 4>>
GetGaussLegendrePoints<4>() {
  std::array<double, 4> x;
  x[0] = 3.39981043584856264792e-01;
  x[1] = -3.39981043584856264792e-01;
  x[2] = 8.61136311594052575248e-01;
  x[3] = -8.61136311594052575248e-01;

  std::array<double, 4> w;
  w[0] = 6.52145154862546142644e-01;
  w[1] = 6.52145154862546142644e-01;
  w[2] = 3.47854845137453857383e-01;
  w[3] = 3.47854845137453857383e-01;

  return std::make_pair(x, w);
}

template <>
inline std::pair<std::array<double, 5>, std::array<double, 5>>
GetGaussLegendrePoints<5>() {
  std::array<double, 5> x;
  x[0] = 0.00000000000000000000e+00;
  x[1] = 5.38469310105683091018e-01;
  x[2] = -5.38469310105683091018e-01;
  x[3] = 9.06179845938663992811e-01;
  x[4] = -9.06179845938663992811e-01;

  std::array<double, 5> w;
  w[0] = 5.68888888888888888883e-01;
  w[1] = 4.78628670499366468030e-01;
  w[2] = 4.78628670499366468030e-01;
  w[3] = 2.36926885056189087515e-01;
  w[4] = 2.36926885056189087515e-01;

  return std::make_pair(x, w);
}

template <>
inline std::pair<std::array<double, 6>, std::array<double, 6>>
GetGaussLegendrePoints<6>() {
  std::array<double, 6> x;
  x[0] = 6.61209386466264513688e-01;
  x[1] = -6.61209386466264513688e-01;
  x[2] = 2.38619186083196908630e-01;
  x[3] = -2.38619186083196908630e-01;
  x[4] = 9.32469514203152027832e-01;
  x[5] = -9.32469514203152027832e-01;

  std::array<double, 6> w;
  w[0] = 3.60761573048138607569e-01;
  w[1] = 3.60761573048138607569e-01;
  w[2] = 4.67913934572691047389e-01;
  w[3] = 4.67913934572691047389e-01;
  w[4] = 1.71324492379170345043e-01;
  w[5] = 1.71324492379170345043e-01;

  return std::make_pair(x, w);
}

template <>
inline std::pair<std::array<double, 7>, std::array<double, 7>>
GetGaussLegendrePoints<7>() {
  std::array<double, 7> x;
  x[0] = 0.00000000000000000000e+00;
  x[1] = 4.05845151377397166917e-01;
  x[2] = -4.05845151377397166917e-01;
  x[3] = 7.41531185599394439864e-01;
  x[4] = -7.41531185599394439864e-01;
  x[5] = 9.49107912342758524541e-01;
  x[6] = -9.49107912342758524541e-01;

  std::array<double, 7> w;
  w[0] = 4.17959183673469387749e-01;
  w[1] = 3.81830050505118944961e-01;
  w[2] = 3.81830050505118944961e-01;
  w[3] = 2.79705391489276667890e-01;
  w[4] = 2.79705391489276667890e-01;
  w[5] = 1.29484966168869693274e-01;
  w[6] = 1.29484966168869693274e-01;

  return std::make_pair(x, w);
}

template <>
inline std::pair<std::array<double, 8>, std::array<double, 8>>
GetGaussLegendrePoints<8>() {
  std::array<double, 8> x;
  x[0] = 1.83434642495649804936e-01;
  x[1] = -1.83434642495649804936e-01;
  x[2] = 5.25532409916328985830e-01;
  x[3] = -5.25532409916328985830e-01;
  x[4] = 7.96666477413626739567e-01;
  x[5] = -7.96666477413626739567e-01;
  x[6] = 9.60289856497536231661e-01;
  x[7] = -9.60289856497536231661e-01;

  std::array<double, 8> w;
  w[0] = 3.62683783378361982976e-01;
  w[1] = 3.62683783378361982976e-01;
  w[2] = 3.13706645877887287338e-01;
  w[3] = 3.13706645877887287338e-01;
  w[4] = 2.22381034453374470546e-01;
  w[5] = 2.22381034453374470546e-01;
  w[6] = 1.01228536290376259154e-01;
  w[7] = 1.01228536290376259154e-01;

  return std::make_pair(x, w);
}

template <>
inline std::pair<std::array<double, 9>, std::array<double, 9>>
GetGaussLegendrePoints<9>() {
  std::array<double, 9> x;
  x[0] = 0.00000000000000000000e+00;
  x[1] = 8.36031107326635794313e-01;
  x[2] = -8.36031107326635794313e-01;
  x[3] = 9.68160239507626089810e-01;
  x[4] = -9.68160239507626089810e-01;
  x[5] = 3.24253423403808929042e-01;
  x[6] = -3.24253423403808929042e-01;
  x[7] = 6.13371432700590397285e-01;
  x[8] = -6.13371432700590397285e-01;

  std::array<double, 9> w;
  w[0] = 3.30239355001259763154e-01;
  w[1] = 1.80648160694857404059e-01;
  w[2] = 1.80648160694857404059e-01;
  w[3] = 8.12743883615744119737e-02;
  w[4] = 8.12743883615744119737e-02;
  w[5] = 3.12347077040002840057e-01;
  w[6] = 3.12347077040002840057e-01;
  w[7] = 2.60610696402935462313e-01;
  w[8] = 2.60610696402935462313e-01;

  return std::make_pair(x, w);
}

template <>
inline std::pair<std::array<double, 10>, std::array<double, 10>>
GetGaussLegendrePoints<10>() {
  std::array<double, 10> x;
  x[0] = 1.48874338981631210881e-01;
  x[1] = -1.48874338981631210881e-01;
  x[2] = 4.33395394129247190794e-01;
  x[3] = -4.33395394129247190794e-01;
  x[4] = 6.79409568299024406207e-01;
  x[5] = -6.79409568299024406207e-01;
  x[6] = 8.65063366688984510759e-01;
  x[7] = -8.65063366688984510759e-01;
  x[8] = 9.73906528517171720066e-01;
  x[9] = -9.73906528517171720066e-01;

  std::array<double, 10> w;
  w[0] = 2.95524224714752870187e-01;
  w[1] = 2.95524224714752870187e-01;
  w[2] = 2.69266719309996355105e-01;
  w[3] = 2.69266719309996355105e-01;
  w[4] = 2.19086362515982044000e-01;
  w[5] = 2.19086362515982044000e-01;
  w[6] = 1.49451349150580593150e-01;
  w[7] = 1.49451349150580593150e-01;
  w[8] = 6.66713443086881375920e-02;
  w[9] = 6.66713443086881375920e-02;

  return std::make_pair(x, w);
}

梯形(Trapezoidal)求积公式

理论

要求$f(x)$在$[a,b]$上的积分，将积分区间等长分成$n$段，则每两个分段点之间的距离$h=\frac{b-a}{n}$，然后如下图(图片来源使用 Trapezoidal Rule（梯形法则）求积分
)进行近似：

则该区间上的积分值就近似等同于每个小梯形的面积之和。

第一个梯形（最左边）的上底$f(x_0)$​​​，下底$f(x_1)$​​​，高为$h=\frac{b-a}{n}$​​​，因此对应的面积为$S_1=(f(x_0)+f(x_1))\ast h/2$​​​。
以此类推，最后一个（最右边）的上底$f(x_{n-1})$​​​，下底$f(x_n)$​​​，高为$h=\frac{b-a}{n}$​​​，因此对应的面积为$S_n=(f(x_{n-1})+f(x_n))\ast h/2$​​​​。
因此，所有梯形的总面积为：
$$\begin{array}{rl}{\int }_a^{b}f(x)dx& \approx \sum _{i=1}^n{S}_i=S_1+S_2+...+S_n\\ & =h/2[f(x_0)+f(x_1)+f(x_1)+f(x_2)+...+f(x_{n-2})+f(x_{n-1})+f(x_{n-1})+f(x_n)]\\ & =\frac{h}{2}[f(x_0)+2\sum _{i=1}^{n-1}f(x_i)+f(x_n)]\\ & =h\ast \sum _{i=1}^{n-1}f(x_i)+0.5\ast h\ast (f(x_0)+f(x_n))\end{array}$$

代码实现

double IntegrateByTrapezoidal(const std::vector<double>& func, const double dx,
                              const std::size_t nsteps) {
  // nsteps表示func集合元素的个数，也就是下标从0到nsteps-1，也就是意味着有nsteps-1个梯形
  // 如参func为f(x_0)到f(x_{nsteps-1})的集合，有nsteps个数，有nsteps-1个梯形
  // dx就是步长(b-a)/(nsteps-1)，也就是公式里面的h
  double sum = 0;
  for (std::size_t i = 1; i + 1 < nsteps; ++i) {
    // 从i=1到i=nsteps-2的累加和
    sum += func[i];
  }
  return dx * sum + 0.5 * dx * (func[0] + func[nsteps - 1]);
}

辛普森(Simpson)求积公式

理论

要求将区间划分成偶数个。图片来自辛普森积分法。

辛普森积分法是一种用抛物线近似函数曲线来求定积分数值解的方法。把积分区间等分成若干段，对被积函数在每一段上使用辛普森公式，根据其在每一段的两端和中点处的值近似为抛物线，逐段积分后加起来，即得到原定积分的数值解。

基本思想就是把复杂的函数$f(x)$​​近似成二次函数。

$$\begin{array}{rl}{\int }_a^{b}f(x)dx& \approx {\int }_a^b(A{x}^2+Bx+C)dx\\ & =\frac{A}{3}(b^3-a^3)+\frac{B}{2}(b^2-a^2)+C(b-a)\\ & =\frac{2A(b^3-a^3)+3B(b^2-a^2)+6C(b-a)}{6}\\ & =\frac{(b-a)[2A(b^2+ab+a^2)+3B(b+a)+6C]}{6}\\ & =\frac{(b-a)(A{a}^2+Ba+C+A{b}^2+Bb+C+A{a}^2+2Aab+A{b}^2+2Bb+2Ba+4C)}{6}\\ & =\frac{(b-a)[f(a)+f(b)+A(a+b{)}^2+2B(a+b)+4C]}{6}\\ & =\frac{(b-a)[f(a)+f(b)+4(A(\frac{a+b}{2}{)}^2+B\frac{a+b}{2}+C)]}{6}\\ & =\frac{(b-a)[f(a)+f(b)+4f(\frac{a+b}{2})]}{6}\end{array}$$
先求定积分${\int }_a^{b}f(x)dx(a<b)$

将闭区间等分成$2n$​​​，即偶数个小区间$[x_i,x_{i+1}](x_i<x_{i+1},x_0=a,x_{2n}=b,i\in [0,2n])$​​​​。在每个小区间上，用抛物线近似函数$f(x)$​​的曲线。每个区间长度为$\frac{b-a}{2n}$​。

以两个区间为基本单位，基本单位长度为$\frac{b-a}{n}$，使用辛普森公式近似求积分：
$$\begin{array}{rl}e.g.{\int }_{x_0}^{x_2}f(x)dx& \approx \frac{(x_2-x_0)[f(x_0)+f(x_2)+4f(x_1)]}{6}\\ & =\frac{(b-a)}{6n}[f(x_0)+4f(x_1)+f(x_2)]\end{array}$$

$$\begin{array}{rl}e.g.{\int }_{x_{2n-2}}^{x_{2n}}f(x)dx& \approx \frac{(x_{2n}-x_{2n-2})[f(x_{2n-2})+f(x_{2n})+4f(x_{2n-1})]}{6}\\ & =\frac{(b-a)}{6n}[f(x_{2n-2})+4f(x_{2n-1})+f(x_{2n})]\end{array}$$

所以累加全部基本单位，可得：
$$\begin{array}{rl}{\int }_a^{b}f(x)dx& \approx {\int }_{x_0}^{x_2}f(x)dx+{\int }_{x_2}^{x_4}f(x)dx+...+{\int }_{x_{2n-2}}^{x_{2n}}f(x)dx\\ & =\frac{b-a}{6n}[f(x_0)+4f(x_1)+f(x_2)+f(x_2)+4f(x_3)+f(x_4)+...+f(x_{2n-2})+4f(x_{2n-1})+f(x_{2n})]\\ & =\frac{b-a}{3\ast 2n}[f(x_0)+f(x_{2n})+4[f(x_1)+f(x_3)+...+f(x_{2n-1})]+2[f(x_2)+f(x_4)+...+f(x_{2n-2})]]\end{array}$$

代码实现

double IntegrateBySimpson(const std::vector<double>& func, const double dx,
                          const std::size_t nsteps) {
  // nsteps表示func集合元素的个数，也就是下标从0到nsteps-1，也就是意味着有nsteps-1个梯形
  // 要求nsteps为奇数，也就是平分成偶数个梯形，nsteps-1个区间
  // 如参func为f(x_0)到f(x_{nsteps-1})的集合，有nsteps个数，有nsteps-1个梯形，即偶数个梯形
  // dx就是步长(b-a)/(nsteps-1)，也就是每个区间的长度
  CHECK_EQ(1, nsteps & 1);
  double sum1 = 0.0;
  double sum2 = 0.0;
  for (std::size_t i = 1; i + 1 < nsteps; ++i) {
    if ((i & 1) != 0) {
      // 奇数求和
      sum1 += func[i];
    } else {
      // 偶数求和
      sum2 += func[i];
    }
  }
  return dx / 3.0 * (4.0 * sum1 + 2.0 * sum2 + func[0] + func[nsteps - 1]);
}

参考文献

使用 Trapezoidal Rule（梯形法则）求积分

自适应辛普森（Simpson）积分

辛普森积分法

数值分析高斯—勒让德积分公式

高斯-勒让德积分公式