【无人机/平衡车/机器人】详解STM32+MPU6050姿态解算—卡尔曼滤波+四元数法+互补滤波——附3个算法源码

1. 卡尔曼滤波

卡尔曼滤波是一种线性最优估计方法，用于估计动态系统的状态。在姿态解算中，我们可以使用卡尔曼滤波来融合陀螺仪和加速度计的数据，以获得更稳定的姿态估计。

以下是一个简单的卡尔曼滤波器实现：

```c
#include "kalman.h"

void Kalman_Init(Kalman_TypeDef *Kalman)
{
    Kalman->P[0][0] = 1;
    Kalman->P[1][1] = 1;
    Kalman->P[2][2] = 1;
    Kalman->Q[0][0] = 0.001;
    Kalman->Q[1][1] = 0.001;
    Kalman->Q[2][2] = 0.001;
    Kalman->R[0][0] = 0.5;
    Kalman->R[1][1] = 0.5;
    Kalman->R[2][2] = 0.5;
}

void Kalman_Update(Kalman_TypeDef *Kalman, float angle_m, float gyro_rate, float dt)
{
    // 预测
    float P00_temp = Kalman->P[0][0] + Kalman->Q[0][0] * dt;
    float P01_temp = Kalman->P[0][1] + Kalman->Q[0][1] * dt;
    float P10_temp = Kalman->P[1][0] + Kalman->Q[1][