void sculm_err_count(float dv_sculm[3], float dv_m[SAMPLE_COUNT][3], float dtheta[SAMPLE_COUNT][3], ...)
{
    va_list valist;
	va_start(valist, dtheta);
    float result_buff[5][3] = {0};
    for(uint8_t i = 0;i < SAMPLE_COUNT-1;i++)
    {
        for(uint8_t j = i+1;j < SAMPLE_COUNT;j++)
        {
            scalar_mult(result_buff[1], (float)va_arg(valist, double), vector_add(result_buff[2], 2, cross_product(result_buff[3], dtheta[i], dv_m[j]), \
                                                                      cross_product(result_buff[4], dv_m[i], dtheta[j])));
            for(uint8_t k = 0;k < 3;k++)result_buff[0][k] += result_buff[1][k];
        }
    }
    memcpy(dv_sculm, result_buff[0], 12);
    va_end(valist);
}

void velocity_position_update(void)
{
    //获取三次姿态数据进行一次多子样速度更新，现在是三子样
    static uint8_t state = 0;

    static float dv_sfm[3] = {0};//比力（测量加速度）引起的速度增量
    static float dv_g[3] = {0};//重力引起的速度增量，以及实验测得的偏差，惯性离心力、科里奥利力不考虑，地面参考系

    static float dv_m[SAMPLE_COUNT][3] = {0};//比力积分
    static float dtheta[SAMPLE_COUNT][3] = {0};//角速度积分
    static float dv_rotm[3] = {0};//旋转效应补偿
    static float dv_sculm[3] = {0};//划桨效应补偿

    static float dv_m_last[2][3] = {0};//上次和上上次的dv_m
    static float dtheta_last[2][3] = {0};//上次和上上次的dtheta

    static float result_buff[4][3];
    
    //这里用于获取多次数据，用于划桨效应补偿
    for(uint8_t i = 0;i < 3;i++)
    {
        dv_m[state][i] = ins.acc[i]*dt_us/1000000.0f;
        dtheta[state][i] = ins.gyro[i]*dt_us/1000000.0f;
    }
    //更新重力引起的速度增量
    dv_g[2] += -ins.g*dt_us/1000000.0f;
    //记录时间
    ins.v_dt_us += dt_us;
    state ++;
    //速度更新
    if(state >= SAMPLE_COUNT)
    {
        //更新划桨补偿项1
        sculm_err_count(dv_sculm, dv_m, dtheta, K1, K2, K2);
        //将三个子样加和,dv_m[0],dtheta[0]本身有数据，所以不用vector_add
        for(uint8_t i = 1;i < SAMPLE_COUNT;i++)
        {
            for(uint8_t j = 0;j < 3;j++)
            {
                dv_m[0][j] += dv_m[i][j];
                dtheta[0][j] += dtheta[i][j];
            }
        }
        //更新旋转补偿项
        scalar_mult(dv_rotm, 0.5, cross_product(result_buff[0], dtheta[0], dv_m[0]));
        //更新划桨补偿项2
        scalar_mult(result_buff[0], P, vector_add(result_buff[1], 2, cross_product(result_buff[2], dtheta_last[0], dv_m[0]), \
                                                                     cross_product(result_buff[3], dv_m_last[0], dtheta[0])));
        for(uint8_t i = 0;i < 3;i++)dv_sculm[i] += result_buff[0][i];
        scalar_mult(result_buff[0], Q, vector_add(result_buff[1], 2, cross_product(result_buff[2], dtheta_last[1], dv_m[0]), \
                                                                     cross_product(result_buff[3], dv_m_last[1], dtheta[0])));
        for(uint8_t i = 0;i < 3;i++)dv_sculm[i] += result_buff[0][i];                                                           
        //更新固联坐标系中速度增量（比力引起速度补偿量）
        vector_add(dv_sfm, 3, dv_m[0], dv_rotm, dv_sculm);
        //更新地面参考系速度
        vector_add(result_buff[0], 3, ins.velocity, left_mult(result_buff[1], ins.attitude_matrix, dv_sfm), dv_g);
        memcpy(ins.velocity, result_buff[0], 12);
        //记录dv_m和dtheta
        vector_add(dv_m_last[1], 1, dv_m_last[0]);
        vector_add(dv_m_last[0], 1, dv_m[0]);
        vector_add(dtheta_last[1], 1, dtheta_last[0]);
        vector_add(dtheta_last[0], 1, dtheta[0]);
        //记录v
        vector_add(ins.velocity_last, 1, ins.velocity);
        //位置更新，梯形积分
        for(uint8_t i = 0;i < 3;i++)
        {
            ins.position[i] += (ins.velocity_last[i]+ins.velocity[i])*ins.v_dt_us/2000000.0f;
        }
        //归零重力增量
        dv_g[2] = 0;
        //归零状态变量
        state = 0;
        //归零速度更新时间
        ins.v_dt_us = 0;
    }
}

这里是纯惯导部分速度位置更新算法，来自INSS0的inertial_navigation.c，划桨补偿较为复杂我就拿出来了。可以看到这里有挺多宏定义，不过不用管，可以看工程。我是每次获取完数据后执行这个函数，在串口中断里，并不是定时中断，所以用了个时间变量，虽然每次获取完数据的时间间隔差不多，但这样还是严谨些。这里是结合了前两次数据的优化三子样，算法可以看这篇算法、代码与测试（完），不过呢，这确实有点杀鸡用牛刀了，我开始也不了解，只是抄算法，最近也在学习，实际上这种低成本MEMS用优化二子样就够了。总之每获取三个数据，就开始计算旋转补偿和划桨补偿，进而积分速度，再梯形积分位置，最后做一些重力增量，状态变量，更新时间等等的归零。