Runge-Kutta Methods Gyroscope

字数统计: 2.7k   |   阅读时长≈ 13 分钟

Runge-Kutta Methods Gyroscope

陀螺仪的观测量是三轴角速度(angular velocity)，对一段时间内的角速度进行积分才能获取到姿态数据。 Runge-Kutta 4th
order normalized method (RK4n)是一种稳定且鲁棒性强的积分算法，相比较于标准线性积分有更高的积分准确度。

数值积分求解微分方程

问题背景

常微分方程(ordinary differential equations)求解，常微分方程是指只有一个变量的微分方程。求解常微分方程的具体任务描述如下，对于给定的常微分方程和初始条件，
$$
\frac{ dy(t)}{dt} = y’(t) = f(y(t),t), \quad \quad {\rm{with;}} y(t_0)=y_0
$$

已知 $y(t_0)=y0$ ，求解当 $t=t_0+h$ 时 $y(t)$ 的值， 其中h是步长(step)。

  • 为什么使用积分求解微分方程

    从微积分基础知识可知：
    $$
    y( t_0 + h) = y( t_0 ) + \int_{t_0}^{t_0 + h} {y’(\lambda )} d\lambda \
    $$
    由于$y’(t)=f(y,t)$ ， 于是：
    $$
    y ( t_0 + h ) = y ( t_0 ) + \int_{t_0}^{t_0 + h} {f( y( \lambda ),\lambda )} d\lambda
    $$

​

​ 从上图中可以看出，
$$
\begin{align}
&y\left( { {t_0} + h} \right) \approx y\left( { {t_0} } \right) + f\left( {y\left( { {t_0} } \right),{t_0} } \right)h \\
&用y^(t)表示y(t)的近似值，则有如下等式:\\
&y^\left( { {t_0} + h} \right) = y\left( { {t_0} } \right) + f\left( {y\left( { {t_0} } \right),{t_0} } \right)h

\end{align}
$$
​ 假设h足够小，则得到的积分近似值和真实值之间的偏差会很小。为了求得下个步长下的近似值我们可以继续使用上面的公式进行 递推：
$$
{y^}( {t_0} + 2h ) = {y^}( {t_0} + h) + f\left(y^\left( t_0 + h \right),t_0 \right)h \
{y^}\left( t_0 + (n + 1)h \right) = {y^}\left( t_0+ nh \right) + f\left( y^\left( t_0 + nh \right),{t_0} \right)h \
$$

  • 使用近似解的原因

    对于微分方程求解，如果能够轻易地求得其方程的解析解，那当然会使用其解析解进行计算，直观且更准确。但实际情况是很多微分方程的解析解没办法求解。例如：
    $$
    {a_0}{ { {d^2}y(t)} \over {d{t^2} } } + {a_1}{ {dy(t)} \over {dt} } + {a_2}y(t) = f\left( t \right), \quad \quad \quad {\rm{with;} } y(t_0)=y_0
    $$

​ 要解上述方程，不同的系数$a_0,a_1,a_2$ 或者不同初始条件下，解析解求取难度不同，因此最简便的方法还是使用数值积分。

​

Euler’s Method (First Order Runge-Kutta)

一阶龙格-库塔(First Order Runge-Kutta)法就是常用的欧拉法(Euler’s Method)。具体方法如下:
$$
\begin{align}
&微分方程:\
&\frac{dy(t)}{dt} = y’\left( t \right) = f(y(t),t)\
& k_1 = y’\left( t \right) = f(y^(t),t)\
&假设时间步长h，我们在t_0附近扩展y(t)\
&y({t_0} + h) = y({t_0}) + y’({t_0})h + y’‘({t_0})\frac{h^2}{2} + \cdots\
& 忽略高阶项:\
&y({t_0} + h) \approx y({t_0}) + y’({t_0})h\
&y^({t_0} + h) = y^*({t_0}) + {k_1}h
\end{align}
$$

Second Order Runge-Kutta

考虑如下一阶线性微分方程：
$$
{ dy(t) \over dt } + 2y(t) = 0\quad \rm{or } \quad { dy(t) \over dt } = - 2y(t) \
$$
初始条件:y(0) = 3,h=0.2。为了方便对比数值积分求解，给出其解析解：$y(t)=3e^{-2t},t\geq0$。

下图中，分别为y(t)和一阶数值积分得到的y*(h)对比，

从图中可以看到，使用一阶数值积分得到的估计值y*(h)和y(h)存在一个偏差，为了减少这种偏差一个很直观的想法就是，使用t=h/2处的倒数作为斜率来估计y*(h)。当t=h/2时，其导数为k2。

如果使用，t=0和t=h中间值的斜率(导数)k2来估计t=h处的值y*(h)，其估计值能更接近真实值。

用t=h/2处的导数作为斜率来估计t=h处的值相比较于一阶方法中用t=0的斜率作为斜率来估计t=h的值，确实能够提高估计结果的准确度，但是在不知道解析解的情况下，我们并不能得到y(h/2)处的准确导数，也即斜率k2的值。

二阶龙格库塔方法具体过程是：

  • 使用一阶龙格-库塔根据t=0的斜率(导数)k1估计t=h/2的近似值y*(h/2)；
  • 将y*(h/2)的值带入微分方程中，求得t=h/2处的斜率k2(k2是y’(h)的近似值)；
  • 再用k2求y(t)在t=h处的估计值y*(h)；

一般的，从t=t0处的估计值到t=t0+h的估计值：

$$
\begin{aligned}
y^{\prime}(t_{0})& =-2y(t_0) && \text{expression for derivative at }t=t_0 \
k_{1}& =-2y^(t_0) && \mathrm{approximate~derivative~at~}t=t_{0} \
y_{1}\left(t_{0}+{\frac{h}{2}}\right)& =y^(t_0)+k_1\frac h2 && \mathrm{intermediate~estimate~of~function~at~}t=t_0+h/2 \
k_{2}& =-2y_1\left(t_0+\frac h2\right) && \mathrm{estimate~of~slope~at~}t=t_0+h/2 \
y(t_{0}+h)& =y(t_0)+y’(t_0)h+y’'(0)\frac{h^2}2+\cdots && \mathrm{Taylor~Series~around~}t=t_{0} \
y(t_{0}+h)& \approx y(t_0)+y^{\prime}(t_0)h && \text{Truncated Taylor Series} \
y^{*}(t_{0}+h)& =y(t_0)+k_2h && \text{estimate of }y(t_0+h)
\end{aligned}
$$

Code

runge_kutta_1th.m

1
2
3
4
5
6
function y_start_th = runge_kutta_1th(f, y_start_t0, h, varargin)
    %
    k1 = f(y_start_t0);
    y_start_th = k1 * h + y_start_t0;
end

runge_kutta_2th.m

1
2
3
4
5
6
7
function y_start_th = runge_kutta_2th(f, y_start_t0, h, varargin)
    k1 = f(y_start_t0);
    ystar_1 = y_start_t0 + k1 * h / 2;
    k2 = f(ystar_1);
    y_start_th = k2 * h + y_start_t0;
end

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
%% Example 3 1th vs 2th

clc;
h = 0.01; % Time step
t = 0:h:2; % t goes from 0 to 2 seconds.
yexact = 3 * exp(-2 * t); % Exact solution (in general we won't know this)

f1 = @(y) - 2 .* y;
h_1 = 0.2;
t_1 = 0:h_1:2;

y_stars_1 = zeros(size(t_1));
y_stars_1(1) = 3;

for k = 1:length(t_1) - 1
    y_stars_1(k + 1) = runge_kutta_1th(f1, y_stars_1(k), h_1);
end

h_2 = 0.2;
t_2 = 0:h_2:2;
y_stars_2 = zeros(size(t_2));
y_stars_2(1) = 3;

for k = 1:length(t_2) - 1
    y_stars_2(k + 1) = runge_kutta_2th(f1, y_stars_2(k), h_2);
end

tcf('tss');
figure('name', 'tss')
hold on
plot(t, yexact, 'LineWidth', 1.5);
plot(t_1, y_stars_1, '-o', 'LineWidth', 1.5)
plot(t_2, y_stars_2, '-o', 'LineWidth', 1.5)
legend('exact', '1th runge kutta', '2th runge kutta')


下图为一阶龙格库塔对比二阶龙格库塔数值积分求解结果：

Fourth Order Runge-Kutta

通过对比1th Runge-Kutta 和 2th Runge-Kutta发现，使用两步斜率估计能获得更高的精度，事实证明使用更多步骤的斜率估计能获取更高的估计精度。因此扩展到四阶Runge-Kutta(4th Runge-Kutta)方法，详细推导参考Applied numerical methods / Brice Carnahan, H.A. Luther, James O. Wilkes.。同2th Runge-Kutta一样，4th Runge-Kutta方法有许多变体，其共同点是都使用斜率的是都使用四个斜率的近似值，下面具体对一种4th Runge-Kutta方法进行说明。
$$
\begin{aligned}
&k_{1} =f(y^(t_0),t_0) \
&k_{2} =f\left(y^(t_0)+k_1\frac h2,t_0+\frac h2\right) \
&k_{3} =f\left(y^(t_0)+k_2\frac{h}{2},t_0+\frac{h}{2}\right) \
&k_{4} =f\left(y^(t_0)+k_3h,t_0+h\right)
\end{aligned}
$$

  • k1是起始时刻的斜率(y’(t0))，同1th中的k1一样；
  • k2是中间时刻点上的的估计斜率，同2th中的k2一样；
  • k3是在k2斜率基础上，另一个中间时刻估计斜率；
  • 最后使用k3，估计出最后时刻的斜率k4；

最后的估计值是上述4个斜率的加权平均得到：
$$
\begin{aligned}y^(t_0+h)&=y^(t_0)+\frac{k_1+2k_2+2k_3+k_4}{6}h=y^(t_0)+\left(\frac{1}{6}k_1+\frac{1}{3}k_2+\frac{1}{3}k_3+\frac{1}{6}k_4\right)h\&=y^(t_0)+mh\quad\text{where }m\text{ is a weighted average slope approximation}\end{aligned}
$$
4th Runge-Kutta方法的核心思想类似于2th Runge-Kutta方法，在区间起始点(t0)和区间终点(t0+h)权重系数一致，4th Runge-Kutta中间点斜率(k2、k3)的权重系数比终点的斜率(k1、k4)大。

可视化四阶龙格-库塔法

对于如下微分方程：
$$
{ {dy(t)} \over {dt} } = y’(t) = -2y(t), \quad \quad {\rm{with;} } y(0)=3
$$
估计t=t0+h的值。

k1,y1

$$
& {k_1} = f({y^*}({t_0}),{t_0}) \
& {k_1} = - 2y({t_0}) = - 2y(0) = - 6
$$

用斜率k1估计一半步长处的值y(h/2)，$y_1(h/2)=y_0+k_1*h/2$

k2,y2

已知y1，可以求得t=t0+h/2处的另一个斜率值k2,
$$
\eqalign{
& {k_2} = f\left( { {y^}({t_0}) + {k_1}{h \over 2},{t_0} + {h \over 2} } \right) \
& {k_2} = - 2y_1 = -4.8
}
$$
用斜率k2估计一半步长处的值y(h/2)，$y_2(h/2)=y_0+k_2h/2$

k3,y3

已知y2，可以求得t=t0+h/2处的另一个斜率值k3。
$$
\eqalign{
& {k_3} = f\left( { {y^}({t_0}) + {k_2}{h \over 2},{t_0} + {h \over 2} } \right) \
& {k_3} = - 2y_2 = -5.04
}
$$
用斜率k3估计步长处的值y(h)，$y_3(h)=y_0+k_3h$

k4,y*

已知y3，可以求得t=t0+h处的斜率值k4。
$$
\eqalign{
& {k_4} = f\left( { {y^*}({t_0}) + {k_3}h,{t_0} + h} \right) \cr \
& {k_4} = - 2y_3 = -3.9840
}
$$
对斜率k*取加权均值作为最后的斜率：
$$
m = {1 \over 6}{k_1} + {1 \over 3}{k_2} + {1 \over 3}{k_3} + {1 \over 6}{k_4}
$$

$$
{y^*}(h) = y(0) + \left( { {1 \over 6}{k_1} + {1 \over 3}{k_2} + {1 \over 3}{k_3} + {1 \over 6}{k_4} } \right)h = y(0) + m \cdot h
$$

Code

runge_kutta_4th.m

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
function y_start_th = runge_kutta_4th(f, y_start_t0, h, varargin)
    % 函数功能:
    %   四阶龙格-库塔法数值积分求解微分方程;
    % 参数:
    %   f,求导函数句柄f(y,t);
    %   y_start_t0,t0时刻对应的y(t0)近似值y*(t0)
    %   h,步长;
    %   varargin,扩展参数
    % 返回:
    %   y_start_th,t0+h时刻对应的y(t0+h)近似值y*(t0+h)
    % 说明:
    %   参考:https://lpsa.swarthmore.edu/NumInt/NumIntFourth.html

    k1 = f(y_start_t0);
    y1 = y_start_t0 + k1 * h / 2;
    k2 = f(y1);
    y2 = y_start_t0 + k2 * h / 2;
    k3 = f(y2);
    y3 = y_start_t0 + k3 * h;
    k4 = f(y3);

    cof = [1, 2, 2, 1] ./ 6;
    y_start_th = y_start_t0 + h * ([k1, k2, k3, k4]) * cof';
end

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
%% Example  1th vs 2th vs 4th Runge-Kutta

clc;
h = 0.01; % Time step
t = 0:h:2; % t goes from 0 to 2 seconds.
yexact = 3 * exp(-2 * t); % Exact solution (in general we won't know this)

f1 = @(y) - 2 .* y;
h_1 = 0.2;
t_1 = 0:h_1:2;

y_stars_1 = zeros(size(t_1));
y_stars_1(1) = 3;

for k = 1:length(t_1) - 1
    y_stars_1(k + 1) = runge_kutta_1th(f1, y_stars_1(k), h_1);
end

h_2 = 0.2;
t_2 = 0:h_2:2;
y_stars_2 = zeros(size(t_2));
y_stars_2(1) = 3;

for k = 1:length(t_2) - 1
    y_stars_2(k + 1) = runge_kutta_2th(f1, y_stars_2(k), h_2);
end

h_4 = 0.2;
t_4 = 0:h_4:2;
y_stars_4 = zeros(size(t_4));
y_stars_4(1) = 3;

for k = 1:length(t_4) - 1
    y_stars_4(k + 1) = runge_kutta_4th(f1, y_stars_4(k), h_4);
end

tcf('tss');
figure('name', 'tss')
hold on
plot(t, yexact, 'LineWidth', 1.5);
plot(t_1, y_stars_1, '-o', 'LineWidth', 1.5)
plot(t_2, y_stars_2, '-o', 'LineWidth', 1.5)
plot(t_4, y_stars_4, '-o', 'LineWidth', 1.5)
legend('exact', '1th runge kutta', '2th runge kutta', '4th runge kutta')
title('Runge-Kutta Methods')

四元数运动学(quaternion kinematics)

四元数运动学微分表达式如下：
$$
\bold{f}(\bold{q},t)=\bold{\dot q} = \frac{1}{2}\Omega(\omega(t))\bold{q}
$$
$\Omega(\omega)$ 是将角速度转换为实斜对称矩阵运算符：
$$
\Omega(\omega)=\left[
\begin{matrix}
&0 \quad &-\omega_x \quad &-\omega_y \quad &-\omega_z \
&\omega_x \quad &0 \quad &\omega_z \quad &-\omega_y \
&\omega_y \quad &-\omega_z \quad &0 \quad & \omega_x \
&\omega_z \quad &\omega_y \quad &-\omega_x \quad &0 \
\end{matrix}
\right]
$$

参考

  1. COMPUTER PROJECT#4 (weebly.com)
  2. 1510.02224.pdf (arxiv.org)
  3. https://byjus.com/maths/runge-kutta-rk4-method/#:~:text=What is Fourth Order RK,for a given point x.
  4. Runge-Kutta 4th Order Method to Solve Differential Equation - GeeksforGeeks
  5. Approximation of Differential Equations by Numerical Integration (swarthmore.edu)
  6. Topic 14.3: 4th-Order Runge Kutta’s Method (Theory) (uwaterloo.ca)
  • 版权声明： 本博客所有文章除特别声明外，著作权归作者所有。转载请注明出处！

扫一扫，分享到微信

微信分享二维码
  • Copyrights © 2020-2023 Wh
  • 访问人数: | 浏览次数:

请我喝杯咖啡吧~

支付宝
微信