在进行vio算法开发时，总是避免不了对imu状态进行积分，当前几种主流的vio算法都是基于Runge-Kutta算法进行数值积分的，其中vins-mono中使用的是中值积分(二阶Runge-Kutta)，msckf中使用的是五阶Runge-Kutta．下面我将列出几种Runge-Kutta算法，随着阶数的提高，积分越精确，实现越困难．
待积分函数
x ˙ = f ( t , x ) \dot{\mathbf{x}}=f(t,\mathbf{x}) x˙=f(t,x)
一阶Runge-Kutta算法(欧拉法)
k 1 = f ( t n , x n ) x n + 1 = x n + Δ t ⋅ k 1 r e s u l t : x n + 1 = x n + Δ t ⋅ f ( t n , x n ) \begin{matrix} k_{1} = f(t_{n},\mathbf{x}_{n}) \\ \mathbf{x}_{n+1}=\mathbf{x}_{n}+\Delta t \cdot k_{1} \\ result: \mathbf{x}_{n+1}=\mathbf{x}_{n}+\Delta t \cdot f(t_{n},\mathbf{x}_{n}) \end{matrix} k1=f(tn,xn)xn+1=xn+Δt⋅k1result:xn+1=xn+Δt⋅f(tn,xn)
二阶Runge-Kutta算法(中值法)
k 1 = f ( t n , x n ) x ( t n + 1 2 Δ t ) = x n + 1 2 Δ t ⋅ k 1 k 2 = f ( t n + 1 2 Δ t , x ( t n + 1 2 Δ t ) ) x n + 1 = x n + Δ t ⋅ k 2 r e s u l t : x n + 1 = x n + Δ t ⋅ f ( t n + 1 2 Δ t , x n + 1 2 Δ t ⋅ f ( t n , x n ) ) \begin{matrix} k_{1}=f(t_{n},\mathbf{x}_{n}) \\ \mathbf{x}(t_{n}+\frac{1}{2}\Delta t)=\mathbf{x}_{n}+\frac{1}{2}\Delta t \cdot k_{1} \\ k_{2}=f(t_{n}+\frac{1}{2}\Delta t,\mathbf{x}(t_{n}+\frac{1}{2}\Delta t)) \\ \mathbf{x}_{n+1}=\mathbf{x}_{n}+\Delta t \cdot k_{2} \\ result: \mathbf{x}_{n+1}=\mathbf{x}_{n}+\Delta t \cdot f(t_{n}+\frac{1}{2}\Delta t,\mathbf{x}_{n}+\frac{1}{2}\Delta t \cdot f(t_{n},\mathbf{x}_{n})) \end{matrix} k1=f(tn,xn)x(tn+21Δt)=xn+21Δt⋅k1k2=f(tn+21Δt,x(tn+21Δt))xn+1=xn+Δt⋅k2result:xn+1=xn+Δt⋅f(tn+21Δt,xn+21Δt⋅f(tn,xn))
四阶Runge-Kutta算法
k 1 = f ( t n , x n ) k 2 = f ( t n + 1 2 Δ t , x n + Δ t 2 k 1 ) k 3 = f ( t n + 1 2 Δ t , x n + Δ t 2 k 2 ) k 4 = f ( t n + Δ t , x n + Δ t ⋅ k 3 ) r e s u l t : x n + 1 = x n + Δ t 6 ( k 1 + 2 k 2 + 2 k 3 + k 4 ) \begin{matrix} k_{1} = f(t_{n},\mathbf{x}_{n}) \\ k_{2} = f(t_{n}+\frac{1}{2}\Delta t,\mathbf{x}_{n}+\frac{\Delta t}{2} k_{1}) \\ k_{3} = f(t_{n}+\frac{1}{2}\Delta t,\mathbf{x}_{n}+\frac{\Delta t}{2} k_{2}) \\ k_{4} = f(t_{n}+\Delta t, \mathbf{x}_{n}+\Delta t \cdot k_{3}) \\ result: \mathbf{x}_{n+1}=\mathbf{x}_{n}+\frac{\Delta t}{6}(k_{1}+2k_{2}+2k_{3}+k_{4}) \end{matrix} k1=f(tn,xn)k2=f(tn+21Δt,xn+2Δtk1)k3=f(tn+21Δt,xn+2Δtk2)k4=f(tn+Δt,xn+Δt⋅k3)result:xn+1=xn+6Δt(k1+2k2+2k3+k4)
五阶Runge-Kutta算法
k 1 = f ( t n , x n ) k 2 = f ( t n + 1 4 Δ t , x n + 1 4 k 1 Δ t ) k 3 = f ( t n + 1 4 Δ t , x n + 1 8 k 1 Δ t + 1 8 k 2 Δ t ) k 4 = f ( t n + 1 2 Δ t , x n − 1 2 k 2 Δ t + k 3 Δ t ) k 5 = f ( t n + 3 4 Δ t , x n + 3 16 k 1 Δ t + 9 16 k 4 Δ t ) k 6 = f ( t n + Δ t , x n − 3 7 k 1 Δ t + 2 7 k 2 Δ t + 12 7 k 3 Δ t − 12 7 k 4 Δ t + 8 7 k 5 Δ t ) r e s u l t : x n + 1 = x n + 1 90 ( 7 k 1 + 32 k 3 + 12 k 4 + 32 k 5 + 7 k 6 ) Δ t \begin{matrix} k_{1} = f(t_{n},\mathbf{x}_{n}) \\ k_{2} = f(t_{n}+\frac{1}{4}\Delta t,\mathbf{x}_{n}+\frac{1}{4} k_{1}\Delta t) \\ k_{3} = f(t_{n}+\frac{1}{4}\Delta t,\mathbf{x}_{n}+\frac{1}{8} k_{1} \Delta t+\frac{1}{8} k_{2}\Delta t) \\ k_{4} = f(t_{n}+\frac{1}{2}\Delta t,\mathbf{x}_{n}-\frac{1}{2} k_{2}\Delta t+k_{3}\Delta t) \\ k_{5} = f(t_{n}+\frac{3}{4}\Delta t,\mathbf{x}_{n}+\frac{3}{16}k_{1}\Delta t+\frac{9}{16}k_{4}\Delta t) \\ k_{6} = f(t_{n}+\Delta{t},\mathbf{x}_{n}-\frac{3}{7}k_{1}\Delta t + \frac{2}{7}k_{2}\Delta t+\frac{12}{7} k_{3}\Delta t - \frac{12}{7}k_{4}\Delta t+\frac{8}{7}k_{5}\Delta t) \\ result: \mathbf{x}_{n+1}=\mathbf{x}_{n}+\frac{1}{90}(7k_{1}+32k_{3}+12k_{4}+32k_{5}+7k_{6})\Delta t \end{matrix} k1=f(tn,xn)k2=f(tn+41Δt,xn+41k1Δt)k3=f(tn+41Δt,xn+81k1Δt+81k2Δt)k4=f(tn+21Δt,xn−21k2Δt+k3Δt)k5=f(tn+43Δt,xn+163k1Δt+169k4Δt)k6=f(tn+Δt,xn−73k1Δt+72k2Δt+712k3Δt−712k4Δt+78k5Δt)result:xn+1=xn+901(7k1+32k3+12k4+32k5+7k6)Δt
7542
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
