卡尔曼滤波算法笔记、例程、原理、应用

1. 卡尔曼滤波算法原理

1.1 核心思想

卡尔曼滤波是一种最优状态估计算法，它的核心思想是：

• 预测：基于上一时刻的状态和运动模型，预测当前时刻的状态

• 更新：结合传感器测量值，修正预测结果，得到最优估计

1.2 数学原理

预测步骤：
x̂(k|k-1) = F(k) * x(k-1|k-1) + B(k) * u(k)  // 状态预测
P(k|k-1) = F(k) * P(k-1|k-1) * F(k)ᵀ + Q(k)  // 协方差预测

更新步骤：
K(k) = P(k|k-1) * H(k)ᵀ * (H(k) * P(k|k-1) * H(k)ᵀ + R(k))⁻¹  // 卡尔曼增益
x(k|k) = x̂(k|k-1) + K(k) * (z(k) - H(k) * x̂(k|k-1))  // 状态更新
P(k|k) = (I - K(k) * H(k)) * P(k|k-1)  // 协方差更新

2. 在项目中的工作原理

2.1 文件关系，层层调用图

main.c
  ↓
Data_Collect.c (数据采集，调用卡尔曼滤波)
  ↓
kalman_filter.c (卡尔曼滤波实现)
  ↓
imu.c (IMU数据处理)

2.2 具体工作流程

姿态估计流程（IMU数据）：

1. IMU传感器 → 原始数据（有噪声）
2. kalman_filter.c → 姿态卡尔曼滤波
3. 输出 → 平滑的姿态角（roll, pitch, yaw）
4. 用于 → PID控制算法

GPS位置估计流程：

1. GPS传感器 → 原始位置数据（有跳变）
2. kalman_filter.c → GPS卡尔曼滤波
3. 输出 → 平滑的位置和速度
4. 用于 → 路径规划和导航

3. 各文件中的具体调用关系

3.1 kalman_filter.h - 头文件

// 定义两个卡尔曼滤波器结构体
typedef struct {
    float x[6];        // 状态向量 [roll, pitch, yaw, ωx, ωy, ωz]
    float P[6][6];     // 协方差矩阵
    float F[6][6];     // 状态转移矩阵
    float H[3][6];     // 观测矩阵
    float Q[6][6];     // 过程噪声协方差
    float R[3][3];     // 观测噪声协方差
    float K[6][3];     // 卡尔曼增益
    float dt;          // 时间间隔
} AttitudeKalmanFilter;

typedef struct {
    float x[6];        // 状态向量 [lat, lon, v_lat, v_lon, heading, v_heading]
    // ... 其他成员
} GPSKalmanFilter;

3.2 imu.c    - IMU数据处理

int imu_decode(void){
    // 1. 初始化卡尔曼滤波器
    if(!kalman_initialized) {
        AttitudeKalman_Init(&attitude_kf, dt);
        kalman_initialized = 1;
        return 1;
    }
    
    // 2. 预测步骤
    AttitudeKalman_Predict(&attitude_kf);
    
    // 3. 获取原始IMU数据
    float roll_meas = (float)(imu_buf[i+3] << 8 | imu_buf[i+2])/32768*180;
    float pitch_meas = (double)(imu_buf[i+7] << 8 | imu_buf[i+6])/32768*180;
    float yaw_meas = (float)(imu_buf[i+5] << 8 | imu_buf[i+4])/32768*180;
    
    // 4. 更新步骤
    AttitudeKalman_Update(&attitude_kf, roll_meas, pitch_meas, yaw_meas);
    
    // 5. 获取滤波后的结果
    float filtered_roll, filtered_pitch, filtered_yaw;
    AttitudeKalman_GetState(&attitude_kf, &filtered_roll, &filtered_pitch, &filtered_yaw);
    
    // 6. 更新系统数据
    board_config.Imu_data.Roll = filtered_roll;
    board_config.Imu_data.Pitch = filtered_pitch;
    board_config.Imu_data.Yaw = filtered_yaw;
}

3.3 Data_Collect.c      - GPS数据处理

uint16_t Inertial_Navigation_Collect(void){
    // 1. 初始化GPS卡尔曼滤波器
    if(!gps_kalman_initialized) {
        GPSKalman_Init(&gps_kf, dt);
        gps_kalman_initialized = 1;
        return 1;
    }
    
    // 2. 预测步骤
    GPSKalman_Predict(&gps_kf);
    
    // 3. 获取原始GPS数据
    double latitude_deg = (double)latitude * 1.0e-7;
    double longitude_deg = (double)longitude * 1.0e-7;
    double heading_angle = (heading * 360.0) / 32768.0;
    
    // 4. 更新步骤
    GPSKalman_Update(&gps_kf, (float)latitude_deg, (float)longitude_deg, (float)heading_angle);
    
    // 5. 获取滤波后的结果
    float filtered_lat, filtered_lon, filtered_heading;
    GPSKalman_GetState(&gps_kf, &filtered_lat, &filtered_lon, &filtered_heading);
    
    // 6. 更新系统数据
    board_config.gps.lat_deg = filtered_lat;
    board_config.gps.lon_deg = filtered_lon;
    board_config.Imu_data.Pitch = -filtered_heading;
}

4. 预期效果对比

4.1 不使用卡尔曼滤波时的问题

原始IMU数据：
Roll: 45.2° → 44.8° → 45.5° → 44.9° → 45.1° (抖动明显)
Pitch: 12.3° → 12.1° → 12.6° → 12.0° → 12.4° (噪声大)
Yaw: 180.5° → 180.2° → 180.8° → 180.1° → 180.6° (不稳定)

原始GPS数据：
Lat: 22.1234567 → 22.1234568 → 22.1234566 → 22.1234569 (跳变)
Lon: 113.1234567 → 113.1234568 → 113.1234566 → 113.1234569 (跳变)

4.2 使用卡尔曼滤波后的效果

滤波后IMU数据：
Roll: 45.0° → 45.0° → 45.0° → 45.0° → 45.0° (平滑稳定)
Pitch: 12.2° → 12.2° → 12.2° → 12.2° → 12.2° (噪声消除)
Yaw: 180.3° → 180.3° → 180.3° → 180.3° → 180.3° (稳定)

滤波后GPS数据：
Lat: 22.1234567 → 22.1234567 → 22.1234567 → 22.1234567 (平滑)
Lon: 113.1234567 → 113.1234567 → 113.1234567 → 113.1234567 (平滑)

5. 实际应用效果

5.1 姿态控制改进

• PID控制更稳定：因为输入数据更平滑，PID输出更稳定

• 减少电机抖动：姿态角数据平滑，电机控制更精确

• 提高控制精度：噪声被过滤，控制误差减小

5.2 路径规划改进

• 轨迹更平滑：GPS数据平滑，路径规划更准确

• 减少路径跳变：消除GPS数据跳变导致的路径突变

• 提高导航精度：位置估计更准确，导航更精确

5.3 系统稳定性提升

• 长时间运行稳定：减少传感器漂移影响

• 环境适应性强：在不同环境下都能保持稳定

• 6. 参数调优原理

  6.1 过程噪声Q的影响

  故障率降低：数据质量提升，系统故障减少

  // 增大Q值 → 更信任观测值 → 响应更快但噪声更大
  kf->Q[0][0] = 0.1f;  // 高噪声，响应快
  
  // 减小Q值 → 更信任预测值 → 响应更慢但更平滑
  kf->Q[0][0] = 0.001f;  // 低噪声，响应慢

  6.2 观测噪声R的影响

  // 增大R值 → 降低对传感器信任度 → 更平滑但响应慢
  kf->R[0][0] = 1.0f;  // 低信任度
  
  // 减小R值 → 提高对传感器信任度 → 响应快但噪声大
  kf->R[0][0] = 0.01f;  // 高信任度

  7. 总结

  卡尔曼滤波在项目中起到数据预处理的作用：

• 输入：有噪声的传感器原始数据

• 处理：通过预测-更新循环，融合历史信息和当前观测

• 输出：平滑、准确的状态估计

• 应用：为PID控制和路径规划提供高质量数据

• 这就像给系统安装了一个"智能过滤器"，能够：

• 自动识别和过滤噪声

• 平滑数据跳变

• 提供最优的状态估计

• 提高整个系统的控制精度和稳定性