在惯性导航以及VIO等实际问题中利用IMU求解位姿需要对IMU测量值进行积分得到需要的位置和姿态,其中主要就是求解微分方程。但之前求解微分方程的解析方法主要是应用于一些简单和特殊的微分方程求解中,对于一般形式的微分方程,一般很难用解析方法求出精确解,只能用数值方法求解。该系列主要介绍一些常用的常微分方程的数值解法,主要包括:
这个系列后面文章会用到前面文章的理论和技术,所以建议按照顺序查看。
简介
在之前常微分方程的数值解法系列中,已经介绍了欧拉法,改进欧拉法以及中值法等多种常微分方程的数值解法。但是之前讲解的方法的局部截断误差相对来说比较大,精度有限,在某些情况下需要更高精度的方法。
本来主要介绍的龙格-库塔法,可以提供更高精度的常微分方程的数值解法。而且龙格-库塔法是常微分方程的数值解法的基本理论,后面讲解的过程中会发现,之前讲解的多种方法都可以归类到龙格-库塔法的体系中。而且不考虑计算量的情况下,理论上是可以构造任意高阶的龙格-库塔公式,这里阶数可以认为和 [常微分方程的数值解法系列一] 常微分方程介绍的截断误差的阶数概念是一致的,阶数越高代表截断误差越小,精度越高。但比较常用的是四阶龙格-库塔公式,后面会给出具体详细的讲解。
基本思想
由微分中值定理可知,存在点ξ \xiξ使得
x ( t i + 1 ) − x ( t i ) Δ t = x ′ ( ξ ) , ξ ∈ ( t i , t i + 1 ) \frac{\mathbf{x}(t_{i+1})-\mathbf{x}(t_i)}{\Delta t} = \mathbf{x}^{\prime}(\xi), \quad \xi \in (t_i, t_{i+1})Δtx(ti+1)−x(ti)=x′(ξ),ξ∈(ti,ti+1)
即
x ( t i + 1 ) = x ( t i ) + Δ t x ′ ( ξ ) = x ( t i ) + Δ t ⋅ f ( ξ , x ( ξ ) ) = x ( t i ) + Δ t k ˉ , (1) \mathbf{x}(t_{i+1}) = \mathbf{x}(t_i) + \Delta t \mathbf{x}^{\prime}(\xi) = \mathbf{x}(t_i) + \Delta t \cdot f(\xi, \mathbf{x}(\xi)) = \mathbf{x}(t_i) + \Delta t \bar{k}, \tag{1}x(ti+1)=x(ti)+Δtx′(ξ)=x(ti)+Δt⋅f(ξ,x(ξ))=x(ti)+Δtkˉ,(1)
其中k ˉ = f ( ξ , x ( ξ ) ) \bar{k} = f(\xi, \mathbf{x}(\xi))kˉ=f(ξ,x(ξ))称为x ( t ) \mathbf{x}(t)x(t)在[ t i , t i + 1 ] [t_i, t_{i+1}][ti,ti+1]上的平均斜率。所以基于这点考虑,只要对平均斜率k ˉ \bar{k}kˉ提供一种近似算法,那么就可以利用x i x_ixi去递推求解x i + 1 x_{i+1}xi+1,这就是龙格-库塔方法的基本思想。
当然不同的近似算法求解精度不同,像之前讲解的向前欧拉法、改进欧拉法和中值法利用龙格-库塔方法思想解释就是分别利用t i t_iti点斜率、t i t_iti点和t i + 1 t_{i+1}ti+1点斜率平均值和t i t_iti点和t i + 1 t_{i+1}ti+1点间中点斜率来近似k ˉ \bar{k}kˉ。
具体方法
一阶
以x ( t ) \mathbf{x}(t)x(t)在t i t_iti点斜率作为x ( t ) \mathbf{x}(t)x(t)在[ t i , t i + 1 ] [t_i, t_{i+1}][ti,ti+1]上的平均斜率k ˉ \bar{k}kˉ,并令x i ≈ x ( t i ) x_{i} \approx \mathbf{x}(t_{i})xi≈x(ti),可得
k ˉ = x ′ ( t i ) = f ( t i , x ( t i ) ) ≈ f ( t i , x i ) \bar{k} = \mathbf{x}^{\prime}(t_i) = f(t_i, \mathbf{x}(t_i)) \approx f(t_{i}, x_{i})kˉ=x′(ti)=f(ti,x(ti))≈f(ti,xi)
所以
x i + 1 = x i + Δ t ⋅ f ( t i , x i ) , (2) x_{i+1}=x_{i} + \Delta t \cdot f(t_{i}, x_{i}), \tag{2}xi+1=xi+Δt⋅f(ti,xi),(2)
这就是[常微分方程的数值解法系列二] 欧拉法文中讲解的向前欧拉法,也叫作一阶龙格-库塔方法。公式( 2 ) (2)(2)就是一阶龙格-库塔公式。
x i x_{i}xi是递推过程中x ( t i ) \mathbf{x}(t_{i})x(ti)的近似值,后面都用x n x_nxn表示x ( t n ) \mathbf{x}(t_n)x(tn)。
二阶
在[ t i , t i + 1 ] [t_i, t_{i+1}][ti,ti+1]上取两点t i , t i + p ( 0 < p ≤ 1 ) t_i,t_{i+p}(0
x i +