目的
在Github上偶然看见一个ORB-VINS的分支ORB-VINS_RK4,工程的地址在https://github.com/RomaTeng/ORB-VINS_RK4。因此决定详细介绍一下龙格-库塔算法的原理和结合SLAM的应用。
Runge-Kutta(龙格-库塔法)
龙格-库塔法是数值计算中的常用方法,用于求解非线性常微分方程的近似解。这种方法是在已知方程斜率的基础上利用
yi
y
i
来推导
yi+1
y
i
+
1
高精度单步法,对于误差也有一定的抑制。这里介绍最经典的4阶定长龙格-库塔方法,也是目前被认为最常用、最有效的算式。(
《Numerical Recipes》
《
N
u
m
e
r
i
c
a
l
R
e
c
i
p
e
s
》
) 。
龙格-库塔法的引入是最经典的欧拉方法:
这个公式就是泰勒展开第一项, f(xn,yn) f ( x n , y n ) 表示 f f 在 处的一阶导数(这种表示方法也是非常奇怪了,居然不是 f˙ f ˙ ,但是还是跟 《Numerical Recipes》 《 N u m e r i c a l R e c i p e s 》 保持一致好了),因此根据佩亚诺(Peano)余项泰勒展开, yn y n 估计的误差是 h2 h 2 的无穷小。
这种方法有两个缺点,一是精度不是很高,二是这种方法的稳定性不是很好。
但是可以考虑类似的步骤,比如在中间架个桥,先按照欧拉方法算一下到终点要在原来的
yn
y
n
上累加的近似值:
于是中间那个点的坐标值可以大概估计一下为 (xn+h2,yn+k12) ( x n + h 2 , y n + k 1 2 ) ,在这个点的导数为 f(xn+h2,yn+k12) f ( x n + h 2 , y n + k 1 2 ) 。
以中间值的导数为为跳板,根据欧拉方法,从起点 yn y n 到达终点 yn+1 y n + 1 需要在中点累加的值:
那么经典的4阶定长R-K算法也差不多,同理,先计算起点到终点的需要累加的近似值,然后获取近似中点1位置坐标,求该点导数,以这个导数为跳板,再次计算起点到终点的需要累加的近似值,接着再求这个近似值的近似中点2的坐标,求导数,最后以这个导数为跳板,计算从起点到终点的近似累加值。最后得到估计的 。
以上 k1、k2、k3、k4 k 1 、 k 2 、 k 3 、 k 4 都是从起点 yn y n 到达终点 yn+1 y n + 1 需要在中点累加的值的估计,因此对 k1、k2、k3、k4 k 1 、 k 2 、 k 3 、 k 4 加权平均,得到(由于在此是为了应用在SLAM系统中,仅获得基本的理解,因此没有详细的推导最后加权是如何计算出来的了):
以上图片均来自
《Numerical Recipes》
《
N
u
m
e
r
i
c
a
l
R
e
c
i
p
e
s
》
。
以下python实现
Runge−Kutta
R
u
n
g
e
−
K
u
t
t
a
用来估计
y=x2
y
=
x
2
函数值的效果。
import numpy as np
import matplotlib.pyplot as plt
# print(plt.style.available)
plt.style.use("seaborn-colorblind")
class Runge_Kutta:
def __init__(self, x_init, y_init, step, times):
self.x_init = x_init
self.y_init = y_init
self.step = step
self.times = times
self.x_coordinate = [x_init]
self.y_predict = [y_init]
self.y_real = [y_init]
self.relative_error = [0]
def derivatives_fun(self, x, y):
return 2*x
def real_fun(self, x):
return x**2
def rf4(self):
for i in range(self.times):
k1 = self.derivatives_fun(self.x_init, self.y_init)
k2 = self.derivatives_fun(self.x_init + self.step / 2, self.y_init + self.step * k1 / 2)
k3 = self.derivatives_fun(self.x_init + self.step / 2, self.y_init + self.step * k2 / 2)
k4 = self.derivatives_fun(self.x_init + self.step, self.y_init + self.step * k3)
y_end = self.y_init + self.step * (k1 + 2 * k2 + 2 * k3 + k4) / 6
# print(y_end)
x_end = self.x_init + self.step
relative_error = abs(y_end - self.real_fun(x_end))
self.x_init = x_end
self.y_init = y_end
self.y_real.append(self.real_fun(x_end))
self.x_coordinate.append(x_end)
self.y_predict.append(y_end)
self.relative_error.append(relative_error)
def main():
# 传入初始位置坐标(x_init, y_init)步长step,迭代次数times
rk4 = Runge_Kutta(0, 0, 0.05, 10)
rk4.rf4()
# print(rk4.y_predict)
plt.figure(1)
plt.subplot(211)
plt.plot(np.array(rk4.x_coordinate), np.array(rk4.y_real), 'b')
plt.plot(np.array(rk4.x_coordinate), np.array(rk4.y_predict), 'o')
plt.subplot(212)
plt.plot(np.array(rk4.x_coordinate), np.array(rk4.relative_error))
plt.show()
if __name__ == '__main__':
main()
红色的曲线是
y=x2
y
=
x
2
的函数曲线,蓝色的点是用龙格库塔法的估计值,非常明显看到估计值基本都落在函数曲线上,并且误差也非常小。
最后概述一下
《Numerical Recipes》
《
N
u
m
e
r
i
c
a
l
R
e
c
i
p
e
s
》
中的评述语:首先需要注意的是不是越高阶就意味着越高的精度,其次结合自适应步长可以更好地发挥龙格库塔算法的优势,并且需要更高的精度时,Bulirsch-Stoer方法和预测-校正法会更加有效。精度要求不高时,龙格库塔算法比较有竞争力。
下一章将会详细介绍龙格库塔算法怎样结合到ORB-VINS系统中。