然后现在发几段我认为有移植意义的代码。

#include "main.h"
#include <math.h>
#include <stdarg.h>
#include <stdlib.h>
#include <string.h>

float* vector_add(float result[3], int num, ...)
{
	//多个向量加
	va_list valist;
	va_start(valist, num);

	memset(result, 0, 12);
	 
	float *vec;
	for(uint8_t i = 0;i < num;i++)
	{
		vec = va_arg(valist, float*);
		for(uint8_t j = 0;j < 3;j++)result[j] += vec[j];
	}

	va_end(valist);

	return result;
}

float* scalar_mult(float result[3], float k, float vec[3])
{
	//数乘
	for(uint8_t i = 0;i < 3;i++)result[i] = k*vec[i];
	return result;
}

float* cross_product(float result[3], float vec1[3] ,float vec2[3])
{
	//叉乘
	result[0] = vec1[1]*vec2[2]-vec1[2]*vec2[1];
	result[1] = vec1[2]*vec2[0]-vec1[0]*vec2[2];
	result[2] = vec1[0]*vec2[1]-vec1[1]*vec2[0];
	return result;
}

float* left_mult(float result[3], float mat[3][3], float vec[3])
{
	//矩阵左乘
	result[0] = mat[0][0]*vec[0]+mat[0][1]*vec[1]+mat[0][2]*vec[2];
	result[1] = mat[1][0]*vec[0]+mat[1][1]*vec[1]+mat[1][2]*vec[2];
	result[2] = mat[2][0]*vec[0]+mat[2][1]*vec[1]+mat[2][2]*vec[2];
	return result;
}

来源INSS0工程中的matrix_count.c全部，这里主要是实现三维向量线性相加，数乘，叉乘，矩阵左乘的函数，这些在惯导解算中较为常用。线性相加用了stdarg.h的不定参数，用于实现多个向量加和。用法是外部定义结果存储的位置，然后参数和返回值都指向那个位置，方便嵌套，节省代码量且更接近于数学公式。

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用优化二子样就够了。总之每获取三个数据，就开始计算旋转补偿和划桨补偿，进而积分速度，再梯形积分位置，最后做一些重力增量，状态变量，更新时间等等的归零。

/*接收
    0xFF ...开始任务
*/
/*发送状态
    0xFF 0x00 ...系统启动
    0xFF 0x01 ...自检通过
    0xFF 0x02 ...任务开始
    0xFF 0x03 ...发射
    0xFF 0x04 ...开伞
    0xFF 0x05 ...着陆
*/
/*发送数据
    0x00 ...欧拉角
    0x01 ...加速度
    0x02 ...角速度
    0x03 ...速度
    0x04 ...位置
*/

无线数据包的包头定义，来自Flight_Controler0的controller.h，一个数据包13个字节，如果飞控接收只需第一个字节，如果发送状态第一个字节0xFF表示状态数据包，第二个字节表示对应状态，发送数据第一个字节表示数据包数据种类，后12个字节储存三个浮点数。

void controller_init(void)
{
    inss_start_read();
    gps_start_read();
    NRF24L01P_init();
    LED_OFF();
    memset(ind.velocity, 0, 12);
    memset(ind.position, 0, 12);
    memset(esd.dh, 0, 8);

    tx_packet[0] = 0xFF;
    tx_packet[1] = 0x00;
    NRF24L01P_send();

    HAL_Delay(200);
}

void controller_selftest(void)
{
    uint8_t test_state;
    tx_packet[0] = 0xFF;
    tx_packet[1] = 0x01;
    while(1)
    {
        test_state = 0;
        //检查惯性导航及环境数据
        if(ins.dt_us > 6000)test_state = 1;
        if(fabs(modulus(ins.acc) - g) > DELTA_a)test_state = 1;
        if(modulus(ins.gyro) > DELTA_g)test_state = 1;
        if(fabs(modulus(esd.mag) - GEOMAG) > DELTA_m)test_state = 1;
        if(esd.t_p_h[2] <= 0)test_state = 1;//这个是防止没刷新到的，只需要再海平面以上工作
        //检查GPS数据
        if(gps.gps_position_state == '0')test_state = 1;
        if(gps.gps_velocity_state == 'N')test_state = 1;
        if(gps.dimension != '3')test_state = 1;
        if(gps.HDOP > 3)test_state = 1;
        if(gps.VDOP > 3)test_state = 1;
        //暂时只需要在北半球，东半球使用
        if(gps.N_S != 'N')test_state = 1;
        if(gps.E_W != 'E')test_state = 1;
        if(test_state == 0 && NRF24L01P_send() == 0)break;
    }
    printf("SELFTEST_PASS\n");
    LED_ON();
}
void controller_record_initial_data(void)
{
    esd.h_0 = esd.t_p_h[2];
    memcpy(gnss.latitude_0, gps.latitude, 12);
    memcpy(gnss.longtitude_0, gps.longitude, 12);
    gnss.altitude_0 = gps.altitude;
}

飞控主板初始化、自检部分，来自Flight_Controler0的controller.c各位可以看看大致的逻辑，以及什么时候发送状态。那些结构体的定义不用管，内容就是字面的，然后想看的翻工程即可。

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滤波的……这里注意的是速度积分用的不是加速度乘时间，而是惯导回传速度的差分，这样能补偿旋转效应和划桨效应的误差，在高动态下更加准确。

void controller_check_event(void)
{
    switch(state)
    {
    case WAIT_FOR_MISSION:
        if(NRF24L01P_receive() == 0)
        {
            if(rx_packet[0] == 0xFF)
            {
                tx_packet[0] = 0xFF;
                tx_packet[1] = 0x02;
                printf("MISSION_START:%u\n",NRF24L01P_send());
                state = WAIT_FOR_LAUNCH;
            }
        }
        break;
    case WAIT_FOR_LAUNCH:
        if(modulus(ins.acc)>1.2*g)
        {
            //发射事件
            ind.t0 = HAL_GetTick();
            tx_packet[0] = 0xFF;
            tx_packet[1] = 0x03;
            printf("LAUNCH:%u\n",NRF24L01P_send());
            state = RISE;
        }
        break;
    case RISE:
        //设定硬性时间标准，防止发射后因为震动立刻开伞
        if(ind.velocity[2] < 0 && ind.Delta_t > 500)
        {
            //到轨迹顶端，开伞
            parachute_open();
            tx_packet[0] = 0xFF;
            tx_packet[1] = 0x04;
            printf("PARACHUTE_DEPLOY:%u\n",NRF24L01P_send());
            state = FALL; 
        }
        break;
    case FALL:
        if(fabs(modulus(ins.acc) - g) < DELTA_a && modulus(ins.gyro) < DELTA_g)
        {
            //落地
            tx_packet[0] = 0xFF;
            tx_packet[1] = 0x05;
            printf("LANDING:%u\n",NRF24L01P_send());
            state = BACK_TO_GROUND;
        }
        break;
    default:
        break;
    }
}

这部分是判断火箭状态以及开伞的代码，来自Flight_Controler0的controller.c，用了状态机，可以看状态转移的条件。

emm……我想也没啥了……总之就这些，毕竟这不是教程向的……希望对大家有帮助。我们会继续搞下一代飞控的，希望下一代飞控能克服这一代飞控的问题。如果没有意外的话……此文完结（

总之还是讲下算法等等吧，毕竟科创上好像没找到，这方面还是有些空白的。但是在此之前我想先讨论下探空火箭飞控的特点，主控和传感器选择等等杂项。

在开始前要说明的是，由于我也没什么经验，不像可以从书上抄的算法，这些都只是我个人的看法，我会给出我得出结论的理由，但不保证正确。而且本文会针对低空低速并使用低成本MEMS传感器的探空火箭进行大量算法的近似，因为论坛几乎所有探空火箭射高都在5km下，速度都在音速以下，而对于能做到更高高度和速度的大佬，似乎也没有看这篇文章的必要（）还有组合导航部分我也在学所以我讲的很粗糙，主要是捷联惯导的算法和组合导航的思路。

飞控分三个部分，导航，航线规划，制导。这里主要讲导航。对探空火箭，航线规划很简单，因为理想轨迹几乎都是直上直下的。而制导我也没什么好讲的，PID为主，不过在探空火箭上怎么调我也没经验，可能以后的飞控才会有，到时候再说吧（也许我能搞个仿真软件。现阶段我会的控制也就是开伞了。

那就先从导航开始。不可否认的是，导航绝对逃不掉惯性导航。因为不论是气压定高还是卫星导航，都做不到惯导的高刷新率和箭体姿态获取。所以，只要不是太简陋的飞控，惯性导航与惯导IMU都是必要的。

接下来的问题是，纯惯导还是组合导航？由于我们使用的是低成本的惯性导航，贵的就算买得起，也不应该用在现阶段事故率高且回收率低的探空火箭上。爱好者用的基本都是mpu60xx系列，这种IMU误差是极大的，惯导会极快发散，一般认为是t的3次方，实际上，即便静基座条件下，高度通道都会出现指数级发散（这个推导有些复杂，且与飞控开发关系较小，如有需要可自行阅读《捷联惯导算法与组合导航原理》第四章），而探空火箭显然比静基座条件恶劣，实际误差可能更大。短时间也许纯惯导可行，但是加上气压，磁场，卫星导航等廉价模块就能获得更高精度和长时间稳定性，这显然是值得的。有人可能会担心卫星导航锁定的问题，然而市面上买的GPS最大速度都有999.9节，约合514m/s（好臭啊），我想能达到这个速度的大佬完全有资格也有必要找相关机构审批了，所以这几乎不是问题。综上，惯性导航，卫星导航，气压定高，磁场定向的组合导航几乎是业余探空火箭的最优解。（各类视觉导航显然不适用于探空火箭，无线电导航至少我感觉难度过高了，当然有大佬会做那加上肯定是好的）

既然已经确定了思路，那么就应该做下硬件的选择了。

卫星导航接收器用市面上卖的足够了，毕竟它的精度与接收器无关。

气压计，磁力计都没啥问题，数据也较为简单，精度是很方便看的，某多多上选也没问题。我建议在低成本情况下完全可以选mpu9250或jy901系列等磁力计与IMU集成的传感器，我最推荐的是jy901b，又进一步集成了气压计。如果有其他产品那也欢迎推荐。不过如果以后要提升IMU精度可能就找不到能集成的了，可能要分开买气压计和磁力计。

其中IMU是最大的坑，后面讲。

接下来是最重要的imu。emm……其实100多的jy901b甚至更贵到两三百的imu和十块不到的mpu6050没有精度上的区别，它们的区别主要还是内置算法。我对imu的误差这块还不是很了解，还在学习，至少下一代我不打算更换imu，再加上论坛里主要也是使用mpu6xxx或mpu9xxx系列的，所以我决定略去这块。

但是，imu内置的算法是很有必要讲的。从我查到的来看，很多市面上的imu、arduino社区的很多方案，CSDN上开源的很多基于stm32的算法都在使用mahony滤波算法而非kalman滤波算法。mahony融合滤波算法是很优秀的算法，兼顾了计算速度和姿态精度，而且基于PID算法的思想很方便上手，很多无人机飞控都在使用该算法。（由于与本文关系不大，仅简单阐述，假设系统加速度为零，角速度积分出的姿态矩阵左乘理想的重力加速度获取预测的加速度计值，与真实测量值的内积结果作为误差，然后向PID那样，对误差做比例P和积分I，进而修正角速度，磁场修正类似）然而，从前面可以看出，mahony算法的缺点是在系统加速度较大及姿态变化剧烈的时候姿态不准确，而显然探空火箭的整个工作流程都如此。所以这个不管在固定翼还是旋翼无人机上优秀的算法实际上不适用于探空火箭。kalman滤波算法也需要利用重力和磁场修正，但是kalman滤波的比值矩阵K是可变的，概统可以证明它能让误差一直最小化。我使用过JY901b和mpu9250加CSDN上某开源的mahony滤波算法进行动态测试，前者在动态下精度仍然够用，后者就相对不行了。还有arduino的mpu6050示例库还有部分CSDN上开源的代码使用的是mpu6xxx内置的dmp硬件解算，这个算法好像没有公开，我不知道具体也无法判断。这里我觉得要不就直接买自带kalman滤波的imu，要不就自己写算法。当然，我也没有做什么实验精确对比，mahony算法精度的确会低些，但是低到什么程度我就不清楚了，如果硬件资源受限用kalman滤波无法满足实时性的话那也不是不能用。

接下来就是飞控的主控了。我只比较熟悉stm32系列，其他单片机我真的不了解。要做加速度校准，旋转和划桨效应补偿，以及组合导航的数据融合，stm32f1是不够用的（至少我写的代码如此，不排除有大佬能优化代码使其满足，但我觉得可能还是不如换个单片机），算力和外设均如此。我在尝试f4系列。

我之前看到有文章中有对使用arduino的争论，由于我对arduino的了解不深，我也无法做什么判断，不过还是建议要不要完全依赖库，至少要看看库的代码，arduino封装程度毕竟比较高，在中断啊，通讯啊，还有之前说的那个解算算法等等，按我查到的资料看似乎比较简单，如果发现无法满足要求要自己写。还有要做旋转和划桨效应补偿，以及组合导航的数据融合的话nano和uno系列看纸面数据都是不够用的，可能要用别的，这些我就不知道了。

再交代下关于软件滤波的事情，比如主控连续检测几次大于1.2g的噪声才判断发射，再比如加速度数据的滤波。主要是这些维特智能在jy901b里做过了，实测完全不需要，但是如果想要的话，加上去也没啥问题，但是注意不要做均值滤波等会影响数据频率的滤波这样不仅效果不明显，还会降低更新频率，进一步降低精度。如果是自己写姿态算法的话，这个就要注意一下了，原始数据需要这样滤波。

接下来就是无线通讯了。我建议是直接买2.4GHz的数传电台，不要相信某些NRF20L01+卖家说的1100m数据传输，我也在找。这里说件事，某多多在光明正大地卖915MHz数传电台，其实这个频段国内不能无证直接用的（）虽然说我们搞kn、ap都是在这种灰色地带，但是……踩在灰色地带的还是越少越好吧……我们也不像远航机玩家那样需要大数据量超远距离传输……2.4G完全合法的模块也能传3km，真的绰绰有余了，超过3km那再改（

好了应该大概都说了，接下来，额可能要明天，开更简化的捷联惯导算法和一点点卡尔曼滤波算法。

现在写一下捷联惯导的算法。再次强调是简化后的，且只给出结论。这些均来自《捷联惯导算法与组合导航原理》。

先从定义开始。

首先是欧拉角。欧拉角是一种直观地描述旋转的方式。欧拉角的定义有很多种，这里只介绍两种。

第一种是工程上最常用的，几乎所有开源工程，惯导文献都使用这种定义的欧拉角。JY901b也是如此。

定义方式如下：

初始时机体xyz轴对准东北天

先绕机体此时的z轴旋转$\psi$，也称YAW，Y，航向角，角度范围-180°~180°

再绕机体此时的x轴旋转$\theta$，也称PITCH，P，俯仰角，角度范围-90°~90°

最后绕机体此时的y轴旋转$\gamma$，也称ROLL，R，横滚角，角度范围-180°~180°

（角度范围不一定是这样，比如有的文献中YAW角范围0~360°，有些直接传回弧度制等等，不过相互转化是很简单的）

由于欧拉角的缺陷，俯仰角接近90°或-90°时，会出现失效问题。表现有很多，比如我之前这篇文章重刚体定点转动的两种可解情况的求解——欧拉-潘索情形与拉格朗日-泊松情形中出现的“章动角为0时分不清自转与进动现象”（当然那篇文章中欧拉角的定义时物理上常用的，和这个有区别），如果使用欧拉角微分方程直接求解姿态，这样还会导致该方程不可解（当然我们不会用这种算法，精度不够）。而且这种欧拉角是相对于东北天坐标系的，不一定适合探空火箭。所以，《捷联惯导算法与组合导航原理》中介绍了一种适用于运载火箭的欧拉角定义，定义如下：

初始时xy轴在弹道平面内，x轴水平，y轴竖直，z轴由右手螺旋法则确定方向

先绕机体此时的z轴转$\theta$ ，角度范围-180°~180°

再绕机体此时的y轴转$\psi$ ，角度范围-90°~90°

最后绕机体此时的x轴转$\gamma$ ，角度范围-180°~180°

（还需要弹道平面与正北方向的夹角）

这样奇异点再与弹道平面垂直处，火箭几乎不可能出现这种情况。

因为现在使用的欧拉角主要还是给人看，计算都用的四元数，imu也基本随便放的，所以我认为第一种方式反而更好，市面上买得到的内置解算算法的imu几乎都是第一种，资源更多，以后用上好的imu，只能自己写算法的时候再用第二种。当然决定自己写姿态算法的话那就无所谓看喜好了。

接下来是姿态矩阵，主要是有点绕，写清楚先。

$C^a_b$是b系到a系的坐标变换矩阵，也是a系到b系的坐标系变换矩阵（过渡矩阵），也就是有：

$\vec{x}_a = C^a_b\vec{x}_b$（def.1）（坐标向量的变换）

$(\hat{i}_a,\hat{j}_a,\hat{k}_a) = (\hat{i}_b,\hat{j}_b,\hat{k}_b) C^a_b$（def.2）（基变换）

一般称固连在机体的坐标系为b系，参考的导航坐标系为n系，称$C^n_b$为姿态矩阵。

接下来是最难理解的四元数，一般用归一化四元数（模为1，即4个分量平方和为1）表示状态。

每个四元数都能写成一个标量加一个矢量的形式$Q = q_0+q_1\hat{i}+q_2\hat{j}+q_3\hat{k} = a+\vec{b}$（这里的虚数单位只看加减与数乘能等效于单位矢量，而且矢量好像就是从这里来的，不过具体我不懂）（def.3）

简单介绍四元数乘法，四元数乘法不满足交换律，满足结合律与消去律。四元数乘号记作$\circ$。分量形式展开即可，只需满足：

$\hat{i}\circ\hat{i} = -1,\hat{j}\circ\hat{j} = -1,\hat{k}\circ\hat{k} = -1$（def.4）

$\hat{i}\circ\hat{j} = \hat{k} ,\hat{j}\circ\hat{k} = \hat{i} ,\hat{k}\circ\hat{i} = \hat{j}$ （def.5）

$\hat{j}\circ\hat{i} = -\hat{k} ,\hat{k}\circ\hat{j} = -\hat{i} ,\hat{i}\circ\hat{k} = -\hat{j}$ （def.6）

更简单的有哈密顿公式，与上面等价：

$\hat{i}\circ\hat{i} = \hat{j}\circ\hat{j} = \hat{k}\circ\hat{k} = \hat{i}\circ\hat{j}\circ\hat{k} = -1$ （def.7）

本文不会涉及除以上外的四元数相关的数学内容。

由欧拉定理，任意转动达到的末状态都能由绕某等效轴的一次定轴转动达到。旋转矢量$\vec{\phi}$，方向为该等效轴的方向，大小为转角，满足右手螺旋为正，反之为负。

所以任意姿态都能由一次定轴转动从最初的东北天转过来，而这些都可以用一个四元数表示。用四元数表示比欧拉角更加便于姿态更新。

可以证明有：

$Q = q_0+q_1\hat{i}+q_2\hat{j}+q_3\hat{k} = \cos\frac{\phi}{2}+\frac{\vec{\phi}}{\phi}\sin\frac{\phi}{2}$（def.8）

其中：

$q_0^2+q_1^2+q_2^2+q_3^2 = 1$（def.9）

定义说完了，接下来直接开始算法的流程。这里为了编号的方便用大写英文字母表示顺序。

该算法基于以下理想条件：传感器无误差，速度，位移不太大（不用考虑惯性离心力与科里奥利力，也不用考虑地球的形状）

一切之前要读取加速度（比力）与角速度数据，而且实际情况要考虑数据融合。不过部分我还在学……

这里使用旋转矢量法更新姿态，兼顾精度与计算速度，这也是工程上最常见的结算方法。

A求解从上次更新到现在的旋转矢量

B更新四元数

C更新姿态矩阵与欧拉角

至此得出姿态，但是由于低成本MEMS精度不够，直接用会漂，要kalman滤波。这里我还在学，希望下一代飞控搞出来后可以写完整版。

D更新速度

E更新位置

至此得出捷联惯导的结果，但是还需要组合导航，这里只说下我的程序里是怎么做的，还是那个原因（

F数据融合

首先是上一次更新到这一次更新的旋转矢量。这里存在圆锥误差，可以理解为有限大角增量不是矢量导致的转动不可交换误差。

使用N子样误差补偿，即在第m-1次更新到第m次更新种，陀螺仪等时间读取了N个值。这里一般不使用角速度，而是角增量，因为光纤陀螺等直接输出的是角增量，而MEMS这些均值平滑后输出角增量和角速度差别不大。

设更新周期是T，t从上一次更新开始计时，则理论上是：

$\Delta\vec{\theta}_{mi} = \int^{\frac{i}{N}T}_{\frac{i-1}{N}T}\vec{\omega}(t)dt$（A1）

实际上不可能做到连续的积分，最简单的长方形积或者梯形积分即可，我这个代码就是前者。（当然下一代还是计划搞成梯形积分）这里带来的不可交换误差会在后面求旋转矢量时补偿。

即：

$\Delta\vec{\theta}_{mi} = \frac{1}{N}T\vec{\omega}_{i}$ （A2）

$\Delta\vec{\theta}_{mi} = \frac{1}{2N}T(\vec{\omega}_{i-1}+\vec{\omega}_{i})$ （A3）

第m-1次更新到第m次更新过程中，第i次（i<N）角增量记为$\Delta\vec{\theta}_{mi}$，旋转矢量记作$\vec{\phi}_m$，这里给出在锥运动条件下优化的N子样算法。（还有在多项式角速度假设下优化的。一般认为前者模型更接近现实且更准确，但是也没法一般性证明，我可以给个例子，还是这篇重刚体定点转动的两种可解情况的求解——欧拉-潘索情形与拉格朗日-泊松情形，结果是自由定点转动下角速度是雅可比椭圆函数形式，而三角函数作为其特殊情况，显然拟合得更好些。而且探空火箭一般有两个相等的主轴惯量，文中证明了无外力矩的情况下精确满足锥运动条件。不过探空火箭绕质心的定点转动无法忽略空气阻力，这些嘛……也没办法严谨处理……只能用不完整的经验归纳）

$\vec{\phi}_m = \sum^\limits{N}_{i=1}\Delta\vec{\theta}_{mi}+(\sum^\limits{N-1}_{i=1}K_{N-i}\Delta\vec{\theta}_{mi})\times\vec{\theta}_{mN}$（A4）

其中：

一子样：无修正项

二子样：$K_1 = \frac{2}{3}$

三子样：$K_1 = \frac{27}{20},K_2 = \frac{9}{20}$

四字样：$K_1 = \frac{214}{105},K_2 = \frac{92}{105},K_3 = \frac{54}{105}$

显式解析式过于复杂，一般更高阶求解都是靠解方程得数值解的，具体见《捷联惯导算法与组合导航原理》。另外也有更优化的联系历史数据的算法，但是我觉得低成本MEMS根本没有必要上。

很容易看出，（A1）式等号右侧第一项即所有角增量求和，第二项即为圆锥误差修正。可以看出，角增量趋于零的时候，总的角增量就趋于旋转矢量。这个算法只能保证在特定的模型下，N趋于无穷时结果趋于真实的旋转矢量。现实确实最接近锥运动模型，但是毕竟不是，而且还有噪声干扰。综合考虑精度和计算速度，实际应用上一般取N=2、3、4。4子样就足够高了，对于低成本MEMS，实际上2子样的算法误差就足够小了（一般保证算法误差小于仪器误差的5%）。

注：角度使用弧度制。

这个公式实际上有不同的表述形式，由于一般读取是等时的，所以都可以化简成式（A4）。

接下来我直接给出1到4子样的公式。

一子样：$\vec{\phi}_m = \Delta\vec{\theta}_{m1}$（A5）

二子样：$\vec{\phi}_m = (\Delta\vec{\theta}_{m1}+\Delta\vec{\theta}_{m2})+\frac{2}{3}\Delta\vec{\theta}_{m1}\times\Delta\vec{\theta}_{m2}$ （A6）

三子样：$\vec{\phi}_m = (\Delta\vec{\theta}_{m1}+\Delta\vec{\theta}_{m2}+\Delta\vec{\theta}_{m3})+(\frac{9}{20}\Delta\vec{\theta}_{m1}+\frac{27}{20}\Delta\vec{\theta}_{m2})\times\Delta\vec{\theta}_{m3}$ （A7）

四子样：$\vec{\phi}_m = (\Delta\vec{\theta}_{m1}+\Delta\vec{\theta}_{m2}+\Delta\vec{\theta}_{m3}+\Delta\vec{\theta}_{m4})+(\frac{54}{105}\Delta\vec{\theta}_{m1}+\frac{92}{105}\Delta\vec{\theta}_{m2}+\frac{214}{105}\Delta\vec{\theta}_{m3})\times\Delta\vec{\theta}_{m4}$ （A8）

好了，那A部分完了，开发时可选（A2）到（A4），建议用二子样，直接翻译成代码就行。

推导和误差分析见《捷联惯导算法与组合导航原理》

接下来是四元数的更新。这里必须介绍各种坐标系，但是嘛……毕竟简化了，所以就说两个，b代指固连在机体上的坐标系，n代指导航使用的参考坐标系，一般选取方向固定东北天的坐标系。然后这里近似认为n系是不运动的，因为之前说了速度不太快，位移不太大的假设，探空火箭又是向上发射的水平位移就更小了。（就算考虑了也顶不住低成本MEMS的爆炸一样的误差）

因此，我们可以按照上述的顺序继续进行，更新四元数，从其能输出四元数来看，市面上能买到的自带算法的imu也是这么做的（如果使用高精度IMU，不近似的话，流程稍有差异，详见《捷联惯导算法与组合导航原理》）。

记$Q^a_b$是a系转到b系的四元数，那么我们实际上是要把已知的$Q^n_{b(m-1)}$ 更新为$Q^n_{b(m)}$ 。

有：

$Q^n_{b(m)} = Q^n_{b(m-1)}\circ Q^{b(m-1)}_{b(m)}$（B1）

$Q^{b(m-1)}_{b(m)} = \cos\frac{\phi_m}{2}+\frac{\vec{\phi}_m}{\phi_m}\sin\frac{\phi_m}{2}$（B2）

只需更具A部分求解出的旋转矢量，利用（B1）和（B2）式就能更新四元数。现在这个四元数已经存储了所有当前姿态信息，接下来要把它转为更直观（欧拉角）和更易于运算（姿态矩阵）的形式。

值得说的是，如果使用JY901b等内置姿态解算程序的imu，可以直接得到四元数，前面的就不要搞了。如果是自己写代码，还有很多要写的。首先就是这个三角函数运算计算机来做肯定需要泰勒展开再截断运算，就算是直接调用库也是这样，再加上计算机的浮点误差，这个四元数很快会偏移出归一化四元数，所以要定时归一化；其次要结合加速度传感器和磁力计的数据进行融合，这个算法就较为复杂了，还是那句话，如果自己写，且不是硬件资源太紧张还是用kalman滤波算法比较好。

推导详见《捷联惯导算法于组合导航原理》

接下来就是把存储了姿态信息的四元数转化为矩阵和欧拉角了。由于四元数直接转欧拉角较为麻烦，一般都是把四元数先转为姿态矩阵，再把它转为欧拉角。

四元数转姿态矩阵的公式为：

$C^n_b = \begin{pmatrix}1-2(q_2^2+q_3^2)&2(q_1q_2-q_0q_3)&2(q_1q_3+q_0q_2)\\2(q_1q_2+q_0q_3)&1-2(q_1^2+q_3^2)&2(q_2q_3+q_0q_2)\\2(q_1q_3-q_0q_2)&2(q_2q_3+q_0q_1)&1-2(q_1^2+q_2^2)\end{pmatrix}$（C1）

记：

$C^n_b = \begin{pmatrix}C_{11}&C_{12}&C_{13}\\C_{21}&C_{22}&C_{23}\\C_{31}&C_{32}&C_{33}\end{pmatrix}$（C2）

为了书写简便，接下来使用的不是数学上的反三角函数，而是c语言<math.h>库中的函数，主要使用asin()和atan2()，前者即反正弦，后者有两个参数，直接能给出以这两个参数为直角坐标系的xy坐标的点对应的极坐标下极角（-180°-180°）。这省去了讨论。还有注意直接给出的是弧度制。

对于工程上最常用的欧拉角，有以下转化：

$\begin{cases}\psi = \begin{cases} -atan2(C_{12},C_{22}),|C_{32}|\leq0.999999\\ 0,|C_{32}|>0.999999\end{cases}\\ \theta = asin(C_{32})\\ \gamma = \begin{cases}-atan2(C_{31},C_{33}),|C_{32}|\leq0.999999\\ atan2(C_{13},C_{11}),|C_{32}|>0.999999 \end{cases} \end{cases}$（C3）

这里的0.999999只是一个随便设定的接近1的值，用于判断是否接近奇点，是的话直接换一套计算方式。

对于运载火箭特化的欧拉角，有以下转化：

此时只要求C31不趋于±1，由于在这种情况下几乎不可能发生，所以可以不用判断，直接这样转化：

$\begin{cases}\theta = atan2(C_{21},C_{11})\\ \psi = -asin(C_{31})\\ \gamma = atan2(C_{32},C_{33})\end{cases}$（C4）

如果使用自带算法的imu，基本都是内置了（C2）（C3）的算法，比如JY901b能同时传回四元数和欧拉角，这块就不用操心了，但是主控里还需要用（C2）来算出姿态矩阵矩阵，并存在一个数组里供后续调用。如果是自己写的话就可以按照这些算法来写，直接翻译成编程语言即可，

姿态矩阵继续参与后续的计算，欧拉角在这套算法下只是作为给人看的数据，传回后地面站利用欧拉角绘图。

推导见《捷联惯导算法与组合导航原理》

接下来是速度更新，需要加速度传感器和角速度传感器的数据。由于之前的近似，可以简化掉很多东西，比如位置对科里奥利力的影响等等……甚至需要考虑的只有重力。

同样，需要在第m-1次更新到第m次采样N次角速度和加速度数据，更新周期为T，t从上一次更新开始计时。由于实际上测得的不是真实加速度，几乎所有文献都用的是比力为名，用f代指，这里就不搞特殊了。

有以下几条：

$\Delta\vec{\theta}_{mi} = \int^{\frac{i}{N}T}_{\frac{i-1}{N}T}\vec{\omega}(t)dt \approx \frac{1}{2N}(\vec{\omega_{i-1}}+\vec{\omega_{i}})$（D1）

$\Delta\vec{\theta}_{m} = \int^{T}_{0}\vec{\omega}(t)dt \approx \sum\limits^N_{i=1}\Delta\vec{\theta}_{mi}$ （D2）

$\Delta\vec{v}_{mi} = \int^{\frac{i}{N}T}_{\frac{i-1}{N}T}\vec{f}(t)dt \approx \frac{1}{2N}(\vec{f_{i-1}}+\vec{f_{i}})$ （D3）

$\Delta\vec{v}_{m} = \int^{T}_{0}\vec{f}(t)dt \approx \sum\limits^N_{i=1}\Delta\vec{v}_{mi}$ （D4）

则，速度更新算法如下，这个算法更新的速度就是n系的速度：

$\vec{v}_m = \vec{v}_{m-1}+C^n_{b(m-1)}(\Delta\vec{v}_m+\Delta\vec{v}_{rot,m}+\Delta\vec{v}_{scul,m})+\Delta\vec{v_g}$（D5）

其中：

$\Delta\vec{v}_g = \begin{pmatrix}0\\0\\-gT\end{pmatrix}$（D6）

为重力导致的速度增量，g为发射当地重力加速度。

$\Delta\vec{v}_{rot,m} = \frac{1}{2}\Delta\vec{\theta}_m\times\Delta\vec{v}_m$（D7）

为旋转效应误差补偿。

$\Delta\vec{v}_{scul,m} = (\sum\limits^{N-1}_{i=1}K_{N-i}\Delta\vec{\theta}_{mi})\times\Delta\vec{v}_{mN}+(\sum\limits^{N-1}_{i=1}K_{N-i}\Delta\vec{v}_{mi})\times\Delta\vec{\theta}_{mN}$（D8）

为划桨效应误差补偿（同样划桨运动优化下），这里的系数KN-i与前面圆锥误差补偿处一样，因为最后得出的方程形式很相似。

这个书上倒是没说N取几比较好，我也没查到相关文献。但是我查到的文献是有超过5子样的，所以也许这个要保留多一点（猜测），不过不论如何，基于低成本MEMS，N取1-4绰绰有余了。这里也是罗列一下：

一子样：$\Delta\vec{v}_{scul,m} = 0$（D9）

二子样：$\Delta\vec{v}_{scul,m} = \frac{2}{3}\Delta\vec{\theta}_{m1}\times\Delta\vec{v}_{m2}+\frac{2}{3}\Delta\vec{v}_{m1}\times\Delta\vec{\theta}_{m2}$ （D10）

三子样：$\Delta\vec{v}_{scul,m} = (\frac{9}{20}\Delta\vec{\theta}_{m1}+\frac{27}{20}\Delta\vec{\theta}_{m2})\times\Delta\vec{v}_{m3}+(\frac{9}{20}\Delta\vec{v}_{m1}+\frac{27}{20}\Delta\vec{v}_{m2})\times\Delta\vec{\theta}_{m3}$ （D11）

四子样：$\Delta\vec{v}_{scul,m} = (\frac{54}{105}\Delta\vec{\theta}_{m1}+\frac{92}{105}\Delta\vec{\theta}_{m2}+\frac{214}{105}\Delta\vec{\theta}_{m3})\times\Delta\vec{v}_{m4}+ (\frac{54}{105}\Delta\vec{v}_{m1}+\frac{92}{105}\Delta\vec{v}_{m2}+\frac{214}{105}\Delta\vec{v}_{m3})\times\Delta\vec{\theta}_{m4}$ （D12）

对于其他的结合历史数据的优化算法，我认为是没有必要的，虽然这个飞控确实用了。

同样的，只要把以上翻译成编程语言即可。

推导见《捷联惯导算法与组合导航原理》。

接下来就是位置了，由于之前的近似，完全不用考虑地球的形状等等，非常简单，再加上速度不会突变，用简单的梯形积分即可。

$\vec{p}_m = \vec{p}_{m-1}+\frac{T}{2}(\vec{v}_{m-1}+\vec{v}_m)$（E1）

要注意的是由于选用的n系是东北天坐标系，这里默认的三轴分别是东北天方向上相对初始点的位移。

emm……至于组合导航，这个我也正在学，所以我就只说下我的程序里的思路吧。就是用惯性导航作为预测，卫星导航和气压定高作为修正，也就是说只需要惯导系统提供速度增量，积分，当卫星数据和气压数据更新后会有标志位，检测到更新则直接对速度和位置的每一个分量进行修正。修正系数基本是估的，也不可变，相当于kalman滤波那里给K蒙了个不可变的对角阵。只能说看上去还是勉强可以用的，但是以后肯定要搞正经的kalman滤波。

算法部分基本说完了。其他部分也没什么好说的。有了这个算法，各种量都有了，那开伞就只需要自己看看用哪些了。然后读取传感器的逻辑也就是基本的用状态机读取数据包的逻辑，传出数据还没做好，实在是没什么可以讲的……

总之那本文到此为止吧，还真在今天内更完了（

我正在开发下一代飞控，吸取这一代飞控的经验教训，硬件上我找了更小更灵敏的卫星导航，无线模块也找了用lora技术的，单片机也改成f4系列了。我正在处理硬件部分，预计是能做成50*100mm的电路板，相比这个小很多了，功能也绝对更强。如果没有意外的话，我会在3月底把它彻底搞好，实验好，开源出来。如果没有意外的话（

文章写的还是很用心，很有逻辑，就是有点考验读者的个人能力。建议你可以把一些设计参数和专业名称给标注出来，比如“SYSTEM_START/SELFTEST_PASS/MISSION_START/LAUNCH/PARACHUTE_DEPLOY/LANDING”不熟悉该硬件的人，尤其是初学者第一眼看上去可能是懵的。电路方面好多都是现成模块的组装，个人建议可以去系统的学一下单片机原理和接口技术，吧原件集成到一块板上面去减重还节省空间又能解决布线和组装的麻烦。还有你的惯导系统可以试一试Gauss-Legendre积分法，长方形积分和梯形积分在长距离检测下累计误差很大。如果之后有设计控制算法和作动装置的需要，可以随时找我合作

楼主用Keil还是CubeMx开发呢？

非常感谢建议，那些状态已经添加了注释。

关于集成化的事情我也考虑过，但是我还是决定在一段时间内保持模块化设计，理由有三，一是本人焊接技术实在不行，即便这样的板子都焊坏了三个焊点，我曾经还干出过把单片机焊反的事情（）而且使用模块还是对坛友DIY友好的；二是辰星引力对飞控开发的目标不是造出一块好用的飞控，而是根据发动机和火箭的要求不断迭代，模块化的设计能在每一次迭代中减小时间的投入，而且也在考虑使用精度更高的MEMSimu，这些是无法集成的；三是我们对飞控的体积和重量要求其实很低，只是这次确实太离谱了。我在画下一代的板子，各传感器使用贴片的模块以省去接线的空间，在保持模块化的情况下体积仍可降到仅用一块50mm*100mm的板子。不过单片机我也在系统性学，如果以后某一代飞控（希望是下一代）的性能足够优秀，能满足论坛中大多数人的要求的话，我会尝试画一块更加集成的板子的，不过我可能不会自己去焊

关于Gauss-Legendre积分法，我刚刚去了解了一下，这主要在组合导航部分应用，在惯导算法中是没有必要的（那本书我还没啃完，我刚翻了一下，在非线性滤波那块看到了，但是我还没学到那里，我还在慢慢学）。在姿态更新与速度更新中，已经使用了多子样算法来补偿积分的误差，而且锥运动补偿和划桨误差补偿是在最贴合现实的物理模型中直接解出来的，精度实际上会更高。而位置的积分对误差的贡献是很小的，这是因为速度不会突变。不仅如此，它对系统整体的误差贡献也是很小的。《捷联惯导算法与组合导航原理》中给出了误差方程，数量级估计可以证明这一点。（不过实在过于冗长，我打不上来）不过在组合导航中是很有用的，我现在也在继续学习。（我这个工程里组合导航确实一坨，问题没这么小）

我是买了个 STM32F103C8T6的套装，里面就是让用 Keil 5标准库，虽然我后来觉得 CubeMX简单不少，但是不习惯，没有库时就从意法半导体官网下（ VScode是怎样开发的．可以给我讲一下吗？）

我使用的vscode加EIDE插件，主要是配置相比keil有点复杂（这个确实三言两语说不清楚，如果真想用的话建议去找教程）。开发方式几乎和keil5+cubesmx一样，只是cubemx里生成代码的地方要改一个配置，实验体验上区别主要是方便用git，还有那个代码颜色和标注更好些，但是整体感觉更复杂。