常微分方程組之龍格-庫塔法

對於方程y'=f(x,y),初始條件:y(x0)=y0,

#include<stdio.h>

FILE *fp=fopen("ex的值.dat","w");

double func(double x)
{
    
    return x;
    
}

void tworder(double x_0, double h, double t_0, double t_n)
{
    
    double n=(t_n-t_0)/h;
    double t=t_0, x=x_0;
    double k1, k2, k3, k4;
    int i;
    
    printf("%f\t%f\n", t, x);
    fprintf(fp,"%f\t%f\n", t, x);
    
    for(i=1;i<=n;i++)
    {
        
        k1=func(x);
        k2=func(x+k1*h/2);
        k3=func(x+k2*h/2);
        k4=func(x+k3*h);
        x=x+(k1+2*k2+2*k3+k4)*h/6;
        t=t+h;
        
        printf("%f\t%f\n", t, x);
        fprintf(fp,"%f\t%f\n", t, x);
        
    }

}

int main()
{   
    
    double h=0.01;
    double x_0=1;
    double t_0=0, t_n=10;
    tworder(x_0, h, t_0, t_n);
    fclose(fp);
    return 0;

}

運行後得到x(t=10)=e^10=22026.465777

用龍格-庫塔法計算的結果並不是絕對精確。但是，只要時間 t取值不要太大，誤差的範圍仍然是可以接受的。

實踐應用：姿態四元數微分方程

% 四元數微分方程的4階龍格庫塔法
% q0:4*1,rotation vector from body-frame to world-frame
% gyro：陀螺儀數據
% T：更新週期
function [ q ] = Quaternion_RungeKutta4( q0,gyro,T)
    q0=Norm_Quaternion(q0); %歸一化
    K1= Quaternion_Diff( gyro,q0);
    q1=Norm_Quaternion(q0+T/2*K1);
    K2 = Quaternion_Diff(gyro,q1);
    q2=Norm_Quaternion(q0+T/2*K2);
    K3 = Quaternion_Diff(gyro,q2);
    q3=Norm_Quaternion(q0+T*K3);
    K4 = Quaternion_Diff(gyro,q3);
    q = q0 + T/6*(K1+2*K2+2*K3+K4);
    q = Norm_Quaternion(q);
end


% 函數功能：四元數微分方程
% 輸    出：四元數的一階導數
% 備    注：連續域
function [ q_diff ] = Quaternion_Diff( gyro,q)

A = [       0, -gyro(1)/2, -gyro(2)/2, -gyro(3)/2;
    gyro(1)/2,          0,  gyro(3)/2, -gyro(2)/2;
    gyro(2)/2, -gyro(3)/2,          0,  gyro(1)/2;
    gyro(3)/2,  gyro(2)/2, -gyro(1)/2,         0];

q_diff = A*q;
end