在惯性导航以及VIO等实际问题中利用IMU求解位姿需要对IMU测量值进行积分得到需要的位置和姿态，其中主要就是求解微分方程。但之前求解微分方程的解析方法主要是应用于一些简单和特殊的微分方程求解中，对于一般形式的微分方程，一般很难用解析方法求出精确解，只能用数值方法求解。该系列主要介绍一些常用的常微分方程的数值解法，主要包括：
- [常微分方程的数值解法系列一] 常微分方程
- [常微分方程的数值解法系列二] 欧拉法
- [常微分方程的数值解法系列三] 改进欧拉法(预估校正法)
- [常微分方程的数值解法系列四] 中值法
- [常微分方程的数值解法系列五] 龙格-库塔(RK4)法
这个系列后面文章会用到前面文章的理论和技术，所以建议按照顺序查看。
简介
[常微分方程的数值解法系列二] 欧拉法和[常微分方程的数值解法系列三] 改进欧拉法(预估校正法)介绍了欧拉法以及改进版欧拉法。中值法思路和改进版欧拉法思路很类似。
向前(显式)欧拉公式
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
⋅
f
(
t
i
+
1
2
,
x
i
+
1
2
)
x_{i+1}=x_{i} + \Delta t \cdot f(t_{i+\frac{1}{2}}, x_{i+\frac{1}{2}})
xi+1=xi+Δt⋅f(ti+21,xi+21)
具体步骤
利用向前(显式)欧拉公式预估一个
x
(
t
i
+
1
2
)
\mathbf{x}(t_{i+\frac{1}{2}})
x(ti+21)的近似值
x
i
+
1
2
=
x
i
+
1
2
Δ
t
⋅
f
(
t
i
,
x
i
)
,
(1)
x_{i+\frac{1}{2}} = x_{i} + \frac{1}{2} \Delta t \cdot f(t_{i}, x_{i}), \tag{1}
xi+21=xi+21Δt⋅f(ti,xi),(1)
利用中点公式求得最终的估计值
x
i
+
1
=
x
i
+
Δ
t
⋅
f
(
t
i
+
1
2
,
x
i
+
1
2
)
,
(2)
x_{i+1}=x_{i} + \Delta t \cdot f(t_{i+\frac{1}{2}}, x_{i+\frac{1}{2}}), \tag{2}
xi+1=xi+Δt⋅f(ti+21,xi+21),(2)
公式
(
1
)
(1)
(1)和公式
(
2
)
(2)
(2)就叫做中值法，也会叫做变形欧拉公式。
截断误差
在[常微分方程的数值解法系列一] 常微分方程文中公式
(
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
⋅
f
(
t
i
+
1
2
,
x
i
+
1
2
)
−
x
(
t
i
+
1
)
,
(5)
\boldsymbol{e}_{i+1} = x_{i} + \Delta t \cdot f(t_{i+\frac{1}{2}}, x_{i+\frac{1}{2}}) - \mathbf{x}(t_{i+1}), \tag{5}
ei+1=xi+Δt⋅f(ti+21,xi+21)−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
2
,
x
i
+
1
2
)
=
f
(
t
i
+
1
2
Δ
t
,
x
i
+
1
2
Δ
t
⋅
f
(
t
i
,
x
i
)
)
=
f
(
t
i
,
x
i
)
+
1
2
Δ
t
f
t
′
(
t
i
,
x
i
)
+
(
1
2
Δ
t
⋅
f
(
t
i
,
x
i
)
)
⋅
f
x
′
(
t
i
,
x
i
)
+
O
(
Δ
t
2
)
=
x
′
(
t
i
)
+
1
2
Δ
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+\frac{1}{2}}, x_{i+\frac{1}{2}}) &= f(t_i + \frac{1}{2}\Delta t, x_{i} + \frac{1}{2} \Delta t \cdot f(t_{i}, x_{i})) \\ &= f(t_i,x_i) + \frac{1}{2}\Delta t f_t^{\prime}(t_i,x_i) + (\frac{1}{2}\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) + \frac{1}{2}\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+21,xi+21)x(ti+1)=f(ti+21Δt,xi+21Δt⋅f(ti,xi))=f(ti,xi)+21Δtft′(ti,xi)+(21Δt⋅f(ti,xi))⋅fx′(ti,xi)+O(Δt2)=x′(ti)+21Δtx′′(ti)+O(Δt2)=x(ti+Δt)=x(ti)+Δtx′(ti)+2!Δt2x′′(ti)+O(Δt3)
带入公式
(
5
)
(5)
(5)可得
e
i
+
1
=
x
i
+
Δ
t
⋅
f
(
t
i
+
1
2
,
x
i
+
1
2
)
−
x
(
t
i
+
1
)
=
x
i
+
Δ
t
(
x
′
(
t
i
)
+
1
2
Δ
t
x
′
′
(
t
i
)
+
O
(
Δ
t
3
)
)
−
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} + \Delta t \cdot f(t_{i+\frac{1}{2}}, x_{i+\frac{1}{2}}) - \mathbf{x}(t_{i+1}) \\ & = x_{i} + \Delta t(\mathbf{x}^{\prime}(t_i) + \frac{1}{2}\Delta t \mathbf{x}^{\prime\prime}(t_i) + \mathbf{O}(\Delta t^3)) - \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+Δt⋅f(ti+21,xi+21)−x(ti+1)=xi+Δt(x′(ti)+21Δtx′′(ti)+O(Δt3))−x(ti)+Δtx′(ti)+2!Δt2x′′(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
2
=
x
i
+
0.1
2
(
x
i
−
2
t
i
x
i
)
=
1.05
x
i
−
0.1
t
i
x
i
x
i
+
1
=
x
i
+
0.1
(
x
i
+
1
2
−
2
t
i
+
1
2
x
i
+
1
2
)
x
0
=
1
\begin{cases} x_{i+\frac{1}{2}} = x_{i} +\frac{0.1}{2}(x_i-\frac{2t_i}{x_i}) = 1.05x_i-\frac{0.1t_i}{x_i} \\ x_{i+1}=x_{i} +0.1(x_{i+\frac{1}{2}} - \frac{2t_{i+\frac{1}{2}}}{x_{i+\frac{1}{2}}}) \\ x_0 = 1 \end{cases}
⎩⎪⎪⎨⎪⎪⎧xi+21=xi+20.1(xi−xi2ti)=1.05xi−xi0.1tixi+1=xi+0.1(xi+21−xi+212ti+21)x0=1
计算得：
| i i i | t i t_i ti | 向前 x i x_i xi | 改进 x i x_i xi | 中值 x i x_i xi | x ( t i ) \mathbf{x}(t_i) x(ti) |
|---|---|---|---|---|---|
| 0 | 0 | 1 | 1 | 1 | 0 |
| 1 | 0.1 | 1.1 | 1.095909 | 1.095476 | 1.095445 |
| 2 | 0.2 | 1.191818 | 1.184097 | 1.183298 | 1.183216 |
| 3 | 0.3 | 1.277138 | 1.266201 | 1.265056 | 1.264911 |
| 4 | 0.4 | 1.358213 | 1.343360 | 1.341859 | 1.341641 |
| 5 | 0.5 | 1.435133 | 1.416402 | 1.414516 | 1.414214 |
| 6 | 0.6 | 1.508966 | 1.485966 | 1.483638 | 1.483240 |
| 7 | 0.7 | 1.580338 | 1.552514 | 1.549702 | 1.549193 |
| 8 | 0.8 | 1.649783 | 1.616475 | 1.613088 | 1.612452 |
| 9 | 0.9 | 1.717779 | 1.678166 | 1.674106 | 1.673320 |
| 10 | 1.0 | 1.784770 | 1.737867 | 1.733012 | 1.732051 |
绘制如下图所示
红色：
x
(
t
i
)
\mathbf{x}(t_i)
x(ti)
绿色：向前
x
i
x_i
xi
蓝色：改进
x
i
x_i
xi
金色：中值
x
i
x_i
xi
在本例中，中值法比改进欧拉法还要好一些，几乎和真值重叠！！！
扫一扫
498
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
