Runge-Kutta（龙格－库塔）在SLAM中的应用（一）

目的

　　在Github上偶然看见一个ORB-VINS的分支ORB-VINS_RK4，工程的地址在https://github.com/RomaTeng/ORB-VINS_RK4。因此决定详细介绍一下龙格-库塔算法的原理和结合SLAM的应用。

Runge-Kutta(龙格-库塔法)

　　龙格-库塔法是数值计算中的常用方法，用于求解非线性常微分方程的近似解。这种方法是在已知方程斜率的基础上利用 $y_i$ 来推导 $y_{i+1}$ 高精度单步法，对于误差也有一定的抑制。这里介绍最经典的4阶定长龙格-库塔方法，也是目前被认为最常用、最有效的算式。（$《Numerical \ Recipes》$） 。
　　龙格-库塔法的引入是最经典的欧拉方法：

$$y_{n+1} = y_{n}+hf(x_{n}, y_{n}) \tag{1}$$ 　　
　　 这个公式就是泰勒展开第一项，$f(x_{n}, y_{n})$ 表示 $f$ 在 $x_{n}$ 处的一阶导数（这种表示方法也是非常奇怪了，居然不是$\dot f$，但是还是跟$《Numerical \ Recipes》$ 保持一致好了），因此根据佩亚诺(Peano）余项泰勒展开，$y_{n}$ 估计的误差是 $h^{2}$ 的无穷小。

　　这种方法有两个缺点，一是精度不是很高，二是这种方法的稳定性不是很好。
　　但是可以考虑类似的步骤，比如在中间架个桥，先按照欧拉方法算一下到终点要在原来的$y_{n}$上累加的近似值：

$$k_{1}=hf(x_{n}, y_{n}) \tag{2}$$ 　
　　 于是中间那个点的坐标值可以大概估计一下为 $(x_{n}+\frac{h}{2}, y_{n}+\frac{k_{1}}{2})$ ，在这个点的导数为$f(x_{n}+\frac{h}{2}, y_{n}+\frac{k_{1}}{2})$ 。
　　 以中间值的导数为为跳板，根据欧拉方法，从起点$y_{n}$到达终点$y_{n+1}$需要在中点累加的值：
$$\begin{equation} k_{2}={h}f(x_{n}+\frac{h}{2}, y_{n}+\frac{k_{1}}{2}) \tag{3} \end{equation}$$ 　　 即
$$\begin{equation} y_{n+1}=y_{n}+k_{2}+O(h^3) \tag{4} \end{equation}$$ 　　 这种方法通过逐阶逐阶迭代消除了误差项，因此在二阶R-K时误差已经是 $h$ 的三阶无穷小了。

　　那么经典的4阶定长R-K算法也差不多，同理，先计算起点到终点的需要累加的近似值，然后获取近似中点1位置坐标，求该点导数，以这个导数为跳板，再次计算起点到终点的需要累加的近似值，接着再求这个近似值的近似中点2的坐标，求导数，最后以这个导数为跳板，计算从起点到终点的近似累加值。最后得到估计的$y_{n+1}$ 。

$$\begin{equation} k_{1}=hf(x_{n}, y_{n}) \tag{5}\\ coordinate\ of\ midpoint\ 1:(x_{n}+\frac{h}{2}, y_{n}+\frac{k_{1}}{2})\\ k_{2}=hf(x_{n}+\frac{h}{2}, y_{n}+\frac{k_{1}}{2})\\ coordinate\ of\ midpoint\ 2:(x_{n}+\frac{h}{2}, y_{n}+\frac{k_{2}}{2})\\ k_{3}=hf(x_{n}+\frac{h}{2}, y_{n}+\frac{k_{2}}{2})\\ coordinate\ of\ approximate\ endpoint: (x_{n}+h, y_{n}+k_{3})\\ k_{4}=hf(x_{n}+h, y_{n}+k_{3}) \end{equation}$$ 　　
　　 以上 $k_{1}、k_{2}、k_{3}、k_{4}$ 都是从起点$y_{n}$到达终点$y_{n+1}$需要在中点累加的值的估计，因此对 $k_{1}、k_{2}、k_{3}、k_{4}$ 加权平均，得到（由于在此是为了应用在SLAM系统中，仅获得基本的理解，因此没有详细的推导最后加权是如何计算出来的了）：
$$\begin{equation} y_{n+1}=y_{n}+\frac{1}{6}k_{1}+\frac{1}{3}k_{2}+\frac{1}{3}k_{3}+\frac{1}{6}k_{4}+O(h_{5}) \tag{6} \end{equation}$$

　　以上图片均来自 $《Numerical \ Recipes》$。
　　以下python实现 $Runge-Kutta$ 用来估计 $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=x^2$ 的函数曲线，蓝色的点是用龙格库塔法的估计值，非常明显看到估计值基本都落在函数曲线上，并且误差也非常小。
　　最后概述一下 $《Numerical \ Recipes》$ 中的评述语：首先需要注意的是不是越高阶就意味着越高的精度，其次结合自适应步长可以更好地发挥龙格库塔算法的优势，并且需要更高的精度时，Bulirsch-Stoer方法和预测-校正法会更加有效。精度要求不高时，龙格库塔算法比较有竞争力。
　　下一章将会详细介绍龙格库塔算法怎样结合到ORB-VINS系统中。