卡尔曼滤波四元数姿态角仿真程序，EKF，UKF算法对比，附带C语言压缩包。 新增完整的单片机UKF实验项目代码！ 基于stm32h750+mpu9250+w25q64实现串口输出姿态角度和保存校准参数。 附带测量一次运行时间的创新功能，通过核心板按键的方式校准IMU单元。 软件工程调用keil中自带库方式组织，大大减小了软件包大小！！

最近在研究姿态解算相关的内容，给大家分享下卡尔曼滤波四元数姿态角仿真程序里EKF（扩展卡尔曼滤波）和UKF（无迹卡尔曼滤波）算法的对比，还附上了C语言压缩包哦，方便大家实操。

EKF与UKF算法初窥

EKF是将非线性系统在当前估计值处进行一阶泰勒展开，从而将非线性问题近似为线性问题，再套用卡尔曼滤波框架。然而这种线性化近似在非线性较强时，会带来较大误差。

卡尔曼滤波四元数姿态角仿真程序，EKF，UKF算法对比，附带C语言压缩包。 新增完整的单片机UKF实验项目代码！ 基于stm32h750+mpu9250+w25q64实现串口输出姿态角度和保存校准参数。 附带测量一次运行时间的创新功能，通过核心板按键的方式校准IMU单元。 软件工程调用keil中自带库方式组织，大大减小了软件包大小！！

UKF则采用UT变换，通过选取一组Sigma点来近似系统的概率分布，能够更准确地处理非线性问题，在很多场景下表现优于EKF 。

单片机UKF实验项目大放送

现在还有完整的单片机UKF实验项目代码！这个项目基于stm32h750 + mpu9250 + w25q64 来实现串口输出姿态角度以及保存校准参数。

核心代码片段与解析

// 初始化MPU9250传感器
void MPU9250_Init(void) {
    // 这里配置MPU9250的寄存器，设置采样率、量程等参数
    I2C_Write_Byte(MPU9250_ADDR, MPU9250_PWR_MGMT_1, 0x01); 
    // 唤醒MPU9250
    delay_ms(100); 
    I2C_Write_Byte(MPU9250_ADDR, MPU9250_SMPLRT_DIV, 0x07); 
    // 设置采样率分频器
    I2C_Write_Byte(MPU9250_ADDR, MPU9250_CONFIG, 0x06); 
    // 设置陀螺仪低通滤波器
    I2C_Write_Byte(MPU9250_ADDR, MPU9250_GYRO_CONFIG, 0x18); 
    // 设置陀螺仪量程为±2000°/s
    I2C_Write_Byte(MPU9250_ADDR, MPU9250_ACCEL_CONFIG, 0x18); 
    // 设置加速度计量程为±16g
}

// UKF核心预测更新部分简化示意
void UKF_Predict_Update(float acc[3], float gyro[3]) {
    // 这里省略复杂的UKF参数初始化等
    // 预测步骤
    for (int i = 0; i < 2 * N + 1; i++) {
        // 计算Sigma点的状态预测值
        x_hat_minus[i] = f(x_hat_plus[i - 1], dt); 
    }
    // 更新步骤
    // 计算卡尔曼增益等操作
    // 根据测量值acc和gyro更新状态估计
}

在MPU9250Init函数里，通过I2C通信对MPU9250的各个寄存器进行配置，让传感器能按我们期望的参数工作。而UKFPredict_Update函数只是简化示意了UKF算法的预测和更新步骤，实际项目中会更复杂，涉及到大量矩阵运算等。

创新功能：测量运行时间

这个项目还附带了测量一次运行时间的创新功能。实现起来也很有意思，通过一些定时器相关的操作就能做到。

// 测量运行时间相关代码示意
TIM_HandleTypeDef TIMx_Init(void) {
    TIM_HandleTypeDef htim;
    htim.Instance = TIMx; 
    htim.Init.Prescaler = 84 - 1; 
    // 预分频器，72MHz / (84) = 1MHz
    htim.Init.CounterMode = TIM_COUNTERMODE_UP; 
    htim.Init.Period = 65535; 
    HAL_TIM_Base_Init(&htim);
    return htim;
}

void Start_Time_Measure(TIM_HandleTypeDef *htim) {
    HAL_TIM_Base_Start(htim); 
}

uint32_t Get_Running_Time(TIM_HandleTypeDef *htim) {
    return __HAL_TIM_GET_COUNTER(htim); 
}

通过TIMxInit函数初始化定时器，StartTimeMeasure启动计时，GetRunning_Time获取运行时间计数值，根据定时器的时钟频率就能换算出实际运行时间。

按键校准IMU单元

通过核心板按键的方式校准IMU单元，这让整个项目更加实用。代码里大概是这样实现按键检测和校准操作的：

// 按键检测与校准相关代码
void Check_Key_For_Calibration(void) {
    if (HAL_GPIO_ReadPin(KEY_GPIO_Port, KEY_Pin) == GPIO_PIN_RESET) {
        // 检测到按键按下
        delay_ms(50); 
        // 消抖
        if (HAL_GPIO_ReadPin(KEY_GPIO_Port, KEY_Pin) == GPIO_PIN_RESET) {
            // 再次确认按键按下
            // 执行IMU校准相关函数
            IMU_Calibration(); 
        }
        while (HAL_GPIO_ReadPin(KEY_GPIO_Port, KEY_Pin) == GPIO_PIN_RESET); 
        // 等待按键释放
    }
}

CheckKeyForCalibration函数不断检测按键状态，当检测到按键按下并经过消抖确认后，就调用IMUCalibration函数执行校准操作。

软件工程组织优化

在软件工程方面，采用调用keil中自带库的方式来组织，大大减小了软件包大小。这是个很实用的技巧，通过合理利用官方提供的库，避免了重复造轮子，也减少了代码冗余。

整个项目无论是对于学习姿态解算，还是研究卡尔曼滤波算法的实际应用，都非常有价值，大家可以下载C语言压缩包，自己动手实践看看哦！