[常微分方程的数值解法系列三] 改进欧拉法(预估校正法)

在惯性导航以及VIO等实际问题中利用IMU求解位姿需要对IMU测量值进行积分得到需要的位置和姿态，其中主要就是求解微分方程。但之前求解微分方程的解析方法主要是应用于一些简单和特殊的微分方程求解中，对于一般形式的微分方程，一般很难用解析方法求出精确解，只能用数值方法求解。该系列主要介绍一些常用的常微分方程的数值解法，主要包括：

这个系列后面文章会用到前面文章的理论和技术，所以建议按照顺序查看。

在[常微分方程的数值解法系列二] 欧拉法文中主要介绍了常用的微分方程的数值解法－向前(显式)欧拉法。但向前(显式)欧拉法虽然计算量小，但是精度比较低；梯形公式是可以提高精度，但是在推导式右端有未知量。所以为了取长补短，在实际计算中是将两种公式的思想结合起来使用。 向前(显式)欧拉公式 x i + 1 = x i + Δ t ⋅ f ( t i , x i ) x_{i+1}=x_{i} + \Delta t \cdot f(t_{i}, x_{i}) xi+1​=xi​+Δt⋅f(ti​,xi​) 梯形公式 x i + 1 = x i + Δ t 2 [ f ( t i , x i ) + f ( t i + 1 , x i + 1 ) ] x_{i+1}=x_{i} +\frac{\Delta t}{2}\left[ f(t_i,x_i) + f(t_{i+1},x_{i+1})\right] xi+1​=xi​+2Δt​[f(ti​,xi​)+f(ti+1​,xi+1​)]

结合上诉向前(显式)欧拉公式和梯形公式，具体步骤如下： 预估 利用向前(显式)欧拉公式预估一个 x ( t i + 1 ) \mathbf{x}(t_{i+1}) x(ti+1​)初始的近似值，也称作预估值 x ^ i + 1 = x i + Δ t ⋅ f ( t i , x i ) , (1) \hat{x}_{i+1} = x_{i} + \Delta t \cdot f(t_{i}, x_{i}), \tag{1} x^i+1​=xi​+Δt⋅f(ti​,xi​),(1) 校正 然后利用这个预估值 x ^ i + 1 \hat{x}_{i+1} x^i+1​作为梯形公式右端 x i + 1 x_{i+1} xi+1​的输入，计算最终的 x ( t i + 1 ) \mathbf{x}(t_{i+1}) x(ti+1​)最终的近似值，也称作校正值 x i + 1 = x i + Δ t 2 [ f ( t i , x i ) + f ( t i + 1 , x ^ i + 1 ) ] , (2) x_{i+1}=x_{i} +\frac{\Delta t}{2}\left[ f(t_i,x_i) + f(t_{i+1},\hat{x}_{i+1})\right], \tag{2} xi+1​=xi​+2Δt​[f(ti​,xi​)+f(ti+1​,x^i+1​)],(2)

上面的预估－校正公式称作为改进欧拉公式。 或者上面的公式可以表示为： { x f = x i + Δ t ⋅ f ( t i , x i ) , x b = x i + Δ t ⋅ f ( t i + 1 , x f ) , x i + 1 = 1 2 ( x f + x b ) , (3) \begin{cases} x_f = x_{i} + \Delta t \cdot f(t_{i}, x_{i}), \\ x_b = x_{i} + \Delta t \cdot f(t_{i+1}, x_{f}), \\ x_{i+1} = \frac{1}{2}(x_f + x_b) \end{cases}, \tag{3} ⎩⎪⎨⎪⎧​xf​=xi​+Δt⋅f(ti​,xi​),xb​=xi​+Δt⋅f(ti+1​,xf​),xi+1​=21​(xf​+xb​)​,(3) 其中 x f x_f xf​表示利用向前(显式)欧拉公式的近似值， x b x_b xb​表示利用向后(隐式)欧拉公式的近似值(利用了 x f x_f xf​)，最后取平均值。 上面利用 f ( t i + 1 , x ^ i + 1 ) f(t_{i+1},\hat{x}_{i+1}) f(ti+1​,x^i+1​)是有误差的，也可通过多次迭代减少误差，具体公式如下 { x i + 1 ( 0 ) = x i + Δ t ⋅ f ( t i , x i ) , x i + 1 ( k + 1 ) = x i + Δ t 2 [ f ( t i , x i ) + f ( t i + 1 , x i + 1 ( k ) ) ] , k = 0 , 1 , 2 , ⋯ , (4) \begin{cases} x_{i+1}^{(0)}=x_{i} + \Delta t \cdot f(t_{i}, x_{i}), \\ x_{i+1}^{(k+1)}=x_{i} +\frac{\Delta t}{2}\left[ f(t_i,x_i) + f(t_{i+1},x_{i+1}^{(k)})\right], \quad k=0,1,2,\cdots \end{cases}, \tag{4} {xi+1(0)​=xi​+Δt⋅f(ti​,xi​),xi+1(k+1)​=xi​+2Δt​[f(ti​,xi​)+f(ti+1​,xi+1(k)​)],k=0,1,2,⋯​,(4)

在[常微分方程的数值解法系列一] 常微分方程文中公式 ( 3 ) (3) (3)可知常微分方程的数值解法的局部截断误差为 e i = x i − x ( t i ) \boldsymbol{e}_i = x_i - \mathbf{x}(t_i) ei​=xi​−x(ti​) 所以对于改进欧拉法来说局部截断误差为： e i + 1 = x i + Δ t 2 [ f ( t i , x i ) + f ( t i + 1 , x i + 1 ) ] − x ( t i + 1 ) , (5) \boldsymbol{e}_{i+1} = x_{i} +\frac{\Delta t}{2}\left[ f(t_i,x_i) + f(t_{i+1},x_{i+1})\right] - \mathbf{x}(t_{i+1}), \tag{5} ei+1​=xi​+2Δt​[f(ti​,xi​)+f(ti+1​,xi+1​)]−x(ti+1​),(5) 对式子右边 f ( t i + 1 , x i + 1 ) f(t_{i+1},x_{i+1}) f(ti+1​,xi+1​)和 x ( t i + 1 ) \mathbf{x}(t_{i+1}) x(ti+1​)分别作泰勒展开 f ( t i + 1 , x i + 1 ) = f ( t i + Δ t , x i + Δ t ⋅ f ( t i , x i ) ) = f ( t i , x i ) + Δ t f t ′ ( t i , x i ) + ( Δ t ⋅ f ( t i , x i ) ) ⋅ f x ′ ( t i , x i ) + O ( Δ t 2 ) = x ′ ( t i ) + Δ t x ′ ′ ( t i ) + O ( Δ t 2 ) x ( t i + 1 ) = x ( t i + Δ t ) = x ( t i ) + Δ t x ′ ( t i ) + Δ t 2 2 ! x ′ ′ ( t i ) + O ( Δ t 3 ) \begin{aligned} f(t_{i+1},x_{i+1}) &= f(t_i + \Delta t, x_{i} + \Delta t \cdot f(t_{i}, x_{i})) \\ &= f(t_i,x_i) + \Delta t f_t^{\prime}(t_i,x_i) + (\Delta t \cdot f(t_{i}, x_{i}))\cdot f_x^{\prime}(t_i,x_i) + \mathbf{O}(\Delta t^2) \\ &=\mathbf{x}^{\prime}(t_i) + \Delta t \mathbf{x}^{\prime\prime}(t_i) + \mathbf{O}(\Delta t^2) \\ \mathbf{x}(t_{i+1}) &= \mathbf{x}(t_i + \Delta t) = \mathbf{x}(t_{i})+\Delta t \mathbf{x}^{\prime}(t_{i}) + \frac{\Delta t^2}{2!} \mathbf{x}^{\prime\prime}(t_{i}) + \mathbf{O}(\Delta t^3) \end{aligned} f(ti+1​,xi+1​)x(ti+1​)​=f(ti​+Δt,xi​+Δt⋅f(ti​,xi​))=f(ti​,xi​)+Δtft′​(ti​,xi​)+(Δt⋅f(ti​,xi​))⋅fx′​(ti​,xi​)+O(Δt2)=x′(ti​)+Δtx′′(ti​)+O(Δt2)=x(ti​+Δt)=x(ti​)+Δtx′(ti​)+2!Δt2​x′′(ti​)+O(Δt3)​ 带入公式 ( 5 ) (5) (5)可得 e i + 1 = x i + Δ t 2 [ f ( t i , x i ) + f ( t i + 1 , x i + 1 ) ] − x ( t i + 1 ) = x i + Δ t 2 [ x ′ ( t i ) + x ′ ( t i ) + Δ t x ′ ′ ( t i ) + O ( Δ t 2 ) ] − x ( t i ) + Δ t x ′ ( t i ) + Δ t 2 2 ! x ′ ′ ( t i ) + O ( Δ t 3 ) = O ( Δ t 3 ) \begin{aligned} \boldsymbol{e}_{i+1} &= x_{i} + \frac{\Delta t}{2}\left[ f(t_i,x_i) + f(t_{i+1},x_{i+1})\right] - \mathbf{x}(t_{i+1}) \\ & = x_{i} + \frac{\Delta t}{2}[\mathbf{x}^{\prime}(t_i) + \mathbf{x}^{\prime}(t_i) + \Delta t \mathbf{x}^{\prime\prime}(t_i) + \mathbf{O}(\Delta t^2)] - \mathbf{x}(t_{i})+\Delta t \mathbf{x}^{\prime}(t_{i}) + \frac{\Delta t^2}{2!} \mathbf{x}^{\prime\prime}(t_{i}) + \mathbf{O}(\Delta t^3) \\ & = \mathbf{O}(\Delta t^3) \end{aligned} ei+1​​=xi​+2Δt​[f(ti​,xi​)+f(ti+1​,xi+1​)]−x(ti+1​)=xi​+2Δt​[x′(ti​)+x′(ti​)+Δtx′′(ti​)+O(Δt2)]−x(ti​)+Δtx′(ti​)+2!Δt2​x′′(ti​)+O(Δt3)=O(Δt3)​ 根据[常微分方程的数值解法系列一] 常微分方程文中局部截断误差定义得 改进欧拉公式具有二阶精度(二阶解法)。

上面求解会用到二元函数的泰勒展开，这里给出三阶二元函数的泰勒展开公式 f ( x + Δ x , y + Δ y ) = f ( x , y ) + Δ x f x ′ ( x , y ) + Δ y f y ′ ( x , y ) + 1 2 ! Δ x f x x ′ ′ ( x , y ) + 1 2 ! Δ x Δ y f x y ′ ′ ( x , y ) + 1 2 ! Δ x Δ y f y x ′ ′ ( x , y ) + 1 2 ! Δ y f y y ′ ′ ( x , y ) + o 3 \begin{aligned} f(x+\Delta x, y + \Delta y) = & f(x, y) +\Delta xf_x^{\prime}(x,y) + \Delta yf_y^{\prime}(x,y) \\ &+ \frac{1}{2!}\Delta xf_{xx}^{\prime \prime}(x,y) + \frac{1}{2!}\Delta x \Delta yf_{xy}^{\prime \prime}(x,y) + \frac{1}{2!}\Delta x \Delta yf_{yx}^{\prime \prime}(x,y) + \frac{1}{2!}\Delta yf_{yy}^{\prime \prime}(x,y) + o^3 \end{aligned} f(x+Δx,y+Δy)=​f(x,y)+Δxfx′​(x,y)+Δyfy′​(x,y)+2!1​Δxfxx′′​(x,y)+2!1​ΔxΔyfxy′′​(x,y)+2!1​ΔxΔyfyx′′​(x,y)+2!1​Δyfyy′′​(x,y)+o3​

分布利用向前欧拉法和改进欧拉法求解初值问题 { x ′ ( t ) = x − 2 t x , 0 ≤ t ≤ 1 x ( t 0 ) = 1 \begin{cases} \mathbf{x}^{\prime}(t)=x - \frac{2t}{x}, \quad 0 \leq t \leq 1 \\ \mathbf{x}({t_0}) = 1 \end{cases} {x′(t)=x−x2t​,0≤t≤1x(t0​)=1​ 求 x ( t = 1 ) \mathbf{x}(t=1) x(t=1)的近似值，步长 Δ t = 0.1 \Delta t = 0.1 Δt=0.1。

这里原函数为 x ( t ) = 1 + 2 t \mathbf{x}(t) =\sqrt{1+2t} x(t)=1+2t ​，用于后面验证结果

解： 向前欧拉公式 { x i + 1 = x i + 0.1 ( x i − 2 t i x i ) = 1.1 x i − 0.2 t i x i x 0 = 1 \begin{cases} x_{i+1}=x_{i} + 0.1(x_i-\frac{2t_i}{x_i}) = 1.1x_i-\frac{0.2t_i}{x_i} \\ x_0 = 1 \end{cases} {xi+1​=xi​+0.1(xi​−xi​2ti​​)=1.1xi​−xi​0.2ti​​x0​=1​ 改进欧拉公式 { x ^ i + 1 = x i + 0.1 ( x i − 2 t i x i ) = 1.1 x i − 0.2 t i x i x i + 1 = x i + 0.1 2 [ x i − 2 t i x i + x ^ i + 1 − 2 t i + 1 x ^ i + 1 ) ] x 0 = 1 \begin{cases} \hat{x}_{i+1} = x_{i} + 0.1(x_i-\frac{2t_i}{x_i}) = 1.1x_i-\frac{0.2t_i}{x_i} \\ x_{i+1}=x_{i} +\frac{0.1}{2}\left[ x_i-\frac{2t_i}{x_i} + \hat{x}_{i+1} - \frac{2t_{i+1}}{\hat{x}_{i+1}})\right]\\ x_0 = 1 \end{cases} ⎩⎪⎪⎨⎪⎪⎧​x^i+1​=xi​+0.1(xi​−xi​2ti​​)=1.1xi​−xi​0.2ti​​xi+1​=xi​+20.1​[xi​−xi​2ti​​+x^i+1​−x^i+1​2ti+1​​)]x0​=1​ 计算得：

绘制如下图所示 红色： x ( t i ) \mathbf{x}(t_i) x(ti​) 绿色：向前 x i x_i xi​ 蓝色：改进 x i x_i xi​ 明显改进欧拉法误差要小于向前欧拉法！！！