常见的数值积分方法（欧拉、中值、龙格-库塔，【常用于IMU中】）

创作时间:

作者:

@小白创作中心

常见的数值积分方法（欧拉、中值、龙格-库塔，【常用于IMU中】）

引用

CSDN

https://blog.csdn.net/hltt3838/article/details/113632866

数值积分是工程计算中的重要工具，特别是在处理IMU（惯性测量单元）数据时，准确的积分方法对于提高系统的稳定性和精度至关重要。本文将介绍三种常见的数值积分方法：欧拉积分、中值积分和龙格-库塔法（RK4），并探讨它们在IMU中的应用。

积分基本概念

设F(x)为函数f(x)的一个原函数，我们把函数f(x)的所有原函数F(x)+C（C为任意常数）叫做函数f(x)的不定积分(indefinite integral)。

在工程上最常见的有三种积分方法：欧拉积分（Euler method）、中值积分（Midpoint method）和龙格－库塔法积分（Runge–Kutta methods）。它们的主要区别在于如何用数值方法模拟一个斜率。

欧拉积分

欧拉方法假设导数在区间内是恒定的，作为一般的RK方法，这对应于单阶段方法，计算初始点的导数，并用它来计算终点的积分值。

中值积分

中值积分法假设导数是间隔中点的导数，并进行一次迭代来计算此中点的fx值。欧拉积分与中值积分都是一阶近似并没有本质不同，二者只是一阶导数所取位置不同，他们的性能也有差别，中点积分有时会稍好一些（带来更快的收敛速度）。

RK4积分(4阶龙格库塔法)

龙格－库塔法（Runge-Kutta methods）是用于非线性常微分方程的解的重要的一类隐式或显式迭代法。在工程中最常用的是四阶龙格－库塔积分，也就是 RK4 积分，它的计算方式如下：

设有如下微分方程：

其中：

k1是时间段开始时的斜率；
k2是时间段中点的斜率，通过欧拉法采用斜率k1来决定y在点tn + h/2的值；
k3也是中点的斜率，但是这次采用斜率k2决定y值；
k4是时间段终点的斜率，其y值用k3决定。

其数学公式如下：

从公式中可以看出两个中点的斜率具有更大的权重。龙格－库塔法的示意图如下，它也是一种更高阶的逼近方法，通常也具有更好的逼近效果，总累计误差为 Δt4 阶。

Runge-Kutta4假定评估值，对于 f() 在间隔的开始，中点，中点的中点和结束。它使用四个阶段迭代计算积分，用四个导数k 1~k 4，顺序获得。然后对这些导数进行加权平均，以获得4阶估计值间隔中的导数。

RK4方法更好地指定为一个小算法而不是一步式公式。龙格-库塔方法的推导基于Taylor展开方法，因而它要求所求的解具有较好的光滑性。如果解的光滑性差，那么，使用四阶龙格-库塔方法求得的数值解，其精度可能反而不如改进的欧拉方法。在实际计算时，应正对问题的具体特点选择适合的算法。对于光滑性不太好的解，最好采用低阶算法而将步长取小。

参考代码

#include "stdio.h"
#include "stdlib.h"

void RKT(t,y,n,h,k,z)
int n;              /*微分方程组中方程的个数，也是未知函数的个数*/
int k;              /*积分的步数(包括起始点这一步)*/
double t;           /*积分的起始点t0*/
double h;           /*积分的步长*/
double y[];         /*存放n个未知函数在起始点t处的函数值,返回时,其初值在二维数组z的第零列中*/
double z[];         /*二维数组,体积为n x k.返回k个积分点上的n个未知函数值*/
{
    extern void Func();             /*声明要求解的微分方程组*/
    int i,j,l;
    double a[4],*b,*d;
    b=malloc(n*sizeof(double));     /*分配存储空间*/
    if(b == NULL)
    {
        printf("内存分配失败\n");
        exit(1);
    }
    d=malloc(n*sizeof(double));     /*分配存储空间*/
    if(d == NULL)
    {
        printf("内存分配失败\n");
        exit(1);
    }
    /*后面应用RK4公式中用到的系数*/
    a[0]=h/2.0;                     
    a[1]=h/2.0;
    a[2]=h; 
    a[3]=h;
    for(i=0; i<=n-1; i++) 
        z[i*k]=y[i];                /*将初值赋给数组z的相应位置*/
    for(l=1; l<=k-1; l++)
    {
        Func(y,d);
        for (i=0; i<=n-1; i++)
            b[i]=y[i];
        for (j=0; j<=2; j++)
        {
            for (i=0; i<=n-1; i++)
            {
                y[i]=z[i*k+l-1]+a[j]*d[i];
                b[i]=b[i]+a[j+1]*d[i]/3.0;
            }
            Func(y,d);
        }
        for(i=0; i<=n-1; i++)
          y[i]=b[i]+h*d[i]/6.0;
        for(i=0; i<=n-1; i++)
          z[i*k+l]=y[i];
        t=t+h;
    }
    free(b);            /*释放存储空间*/
    free(d);            /*释放存储空间*/
    return;
}
main()
{
    int i,j;
    double t,h,y[3],z[3][11];
    y[0]=-1.0; 
    y[1]=0.0; 
    y[2]=1.0;
    t=0.0; 
    h=0.01;
    RKT(t,y,3,h,11,z);
    printf("\n");
    for (i=0; i<=10; i++)           /*打印输出结果*/
    {
        t=i*h;
        printf("t=%5.2f\t   ",t);
        for (j=0; j<=2; j++)
          printf("y(%d)=%e  ",j,z[j][i]);
        printf("\n");
    }
}

void Func(y,d)
double y[],d[];
{
    d[0]=y[1];      /*y0'=y1*/
    d[1]=-y[0];     /*y1'=y0*/
    d[2]=-y[2];     /*y2'=y2*/
    return;
}

参考文章附录A:四元数误差状态卡尔曼滤波
参考亮哥的博客说明:http://www.xinliang-zhong.vip/msckf_notes/

热门推荐

XO酱是什么做的？两种经典配方详解