常微分方程组之龙格-库塔法

对于方程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