void controller_data_integrated(void)
{
    if(ins_e_refreshed == 1)
    {
        //姿态直接用惯性导航数据
        memcpy(ind.eular_angle, ins.eular_angle, 12);
        ins_e_refreshed = 0;
    }
    if(ins_v_refreshed == 1)
    {
        //惯导速度更新，预测速度和位置
        for(uint8_t i = 0;i < 3;i++)
        {
            ind.velocity[i] = ind.velocity[i] + (ins.velocity[i] - ins.velocity_last[i]);
        }
        memcpy(ins.velocity_last, ins.velocity, 12);
        //位置预测
        ins.t[0] = HAL_GetTick();
        dt = (ins.t[0]-ins.t[1])/1000.0f;
        ins.t[1] = ins.t[0];
        for(uint8_t i = 0;i < 3;i++)
        {
            ind.position[i] = ind.position[i] + ind.velocity[i]*dt;
        }

        ins_v_refreshed = 0;
    }
    if(esd_h_refreshed == 1)
    {
        //气压传感器高度更新，修正位置项
        esd.dh[0] = (esd.t_p_h[2] - esd.h_0)/100.0f;

        if(fabs(esd.dh[0]-esd.dh[1]) < 5)
        {
            ind.position[2] = ind.position[2] + K_h*(esd.dh[0] - ind.position[2]);
            esd.dh[1] = esd.dh[0];
        }
        esd_h_refreshed = 0;
    }
    if(gps_v_refreshed == 1)
    {
        //GPS速度更新，修正速度项
        gnss.velocity_g[0] = gps.velocity_ground*sinf(gps.angle*3.1415f/180.0f)/3.6f;
        gnss.velocity_g[1] = gps.velocity_ground*cosf(gps.angle*3.1415f/180.0f)/3.6f;

        ind.velocity[0] = ind.velocity[0]+K_gps_vxy*(gnss.velocity_g[0]-ind.velocity[0]);
        ind.velocity[1] = ind.velocity[1]+K_gps_vxy*(gnss.velocity_g[1]-ind.velocity[1]);

        gps_v_refreshed = 0;
    }
    if(gps_p_refreshed == 1)
    {
        //GPS位置更新，修正位置项
        gnss.position[0] = Re*3.1415f*(gps.longitude[0]+gps.longitude[1]/60+gps.longitude[2]/3600-\
                    gnss.longtitude_0[0]-gnss.longtitude_0[1]/60-gnss.longtitude_0[2]/3600)/180;
        gnss.position[1] = Re*3.1415f*(gps.latitude[0]+gps.latitude[1]/60+gps.latitude[2]/3600-\
                    gnss.latitude_0[0]-gnss.latitude_0[1]/60-gnss.latitude_0[2]/3600)/180;   
        gnss.position[2] = gps.altitude - gnss.altitude_0;
        
        ind.position[0] = ind.position[0] + K_gps_pxy*(gnss.position[0] - ind.position[0]);
        ind.position[1] = ind.position[1] + K_gps_pxy*(gnss.position[1] - ind.position[1]);
        ind.position[2] = ind.position[2] + K_gps_pz*(gnss.position[2] - ind.position[2]);

        gps_p_refreshed = 0;
    }
    if(state == WAIT_FOR_MISSION)
    {
        memset(ind.velocity, 0, 12);
        memset(ind.position, 0, 12);
    }
}

最核心的组合导航算法，来自Flight_Controler0的controller.c，就是惯性导航作为预测，每当气压、卫星导航数据到来时进行修正，这里的K全是宏定义，然后基本是蒙的估计的，反正试了很多次，实验效果不错，当然下一代肯定会搞正经的kalman滤波的……这里注意的是速度积分用的不是加速度乘时间，而是惯导回传速度的差分，这样能补偿旋转效应和划桨效应的误差，在高动态下更加准确。