4. 卡尔曼滤波在姿态估计中的应用

任务说明

本案例实现一个基于卡尔曼滤波的一维姿态角估计器。输入为陀螺仪角速度（单位：°/s）与加速度计解算出的俯仰角（单位：°），输出为融合后的最优俯仰角估计值。卡尔曼滤波通过动态调整陀螺仪积分与加速度计观测的权重，在抑制高频噪声的同时消除积分漂移。

核心算法与实现

系统采用线性离散卡尔曼滤波模型。状态量为俯仰角 θ，控制量为陀螺仪角速度 ω，观测量为加速度计俯仰角 θ_acc。状态方程与观测方程如下：

θ_k = θ_{k-1} + ω·Δt + w_k    （过程噪声 w ~ N(0, Q)）
z_k = θ_k + v_k                （观测噪声 v ~ N(0, R)）

滤波递推分为预测与更新两步。预测阶段根据陀螺仪积分推算先验估计与先验误差协方差；更新阶段根据卡尔曼增益融合加速度计观测，得到后验估计。实现中采用固定时间步长 dt=0.01s（即100Hz采样），过程噪声协方差 Q=0.001，观测噪声协方差 R=0.1，这两个参数可根据实际传感器噪声特性调整。

#include <stdio.h>
#include <math.h>

typedef struct {
    float theta;      // 后验估计：俯仰角（°）
    float P;          // 后验误差协方差
    float Q;          // 过程噪声协方差
    float R;          // 观测噪声协方差
    float dt;         // 采样时间间隔（s）
} Kalman1D;

void kalman_init(Kalman1D *kf, float init_theta, float init_P, float Q, float R, float dt) {
    kf->theta = init_theta;
    kf->P = init_P;
    kf->Q = Q;
    kf->R = R;
    kf->dt = dt;
}

float kalman_update(Kalman1D *kf, float gyro_rate, float acc_theta) {
    // 预测：陀螺仪积分
    float theta_pred = kf->theta + gyro_rate * kf->dt;
    float P_pred = kf->P + kf->Q;

    // 更新：融合加速度计观测
    float K = P_pred / (P_pred + kf->R);
    kf->theta = theta_pred + K * (acc_theta - theta_pred);
    kf->P = (1.0f - K) * P_pred;

    return kf->theta;
}

int main() {
    Kalman1D kf;
    kalman_init(&kf, 0.0f, 1.0f, 0.001f, 0.1f, 0.01f);

    // 模拟输入：陀螺仪角速度（°/s）与加速度计俯仰角（°）
    float gyro_data[] = {2.0f, 2.1f, 1.9f, 2.0f, -1.0f, -1.1f, -0.9f, -1.0f};
    float acc_data[]  = {1.8f, 2.3f, 1.5f, 2.6f, -0.8f, -1.5f, -0.3f, -1.2f};
    int n = sizeof(gyro_data) / sizeof(gyro_data[0]);

    printf("步数\t陀螺仪(°/s)\t加速度计(°)\t滤波输出(°)\n");
    for (int i = 0; i < n; i++) {
        float est = kalman_update(&kf, gyro_data[i], acc_data[i]);
        printf("%d\t%.2f\t\t%.2f\t\t%.2f\n", i+1, gyro_data[i], acc_data[i], est);
    }
    return 0;
}

数据流与滤波效果示意

编译与运行

gcc -o kalman_attitude kalman_attitude.c -lm
./kalman_attitude

验证方法与判定标准

程序运行后输出8步滤波结果。观察第1～4步（陀螺仪角速度约+2°/s）与第5～8步（角速度约−1°/s）的滤波输出：

• 跟踪能力：滤波输出应平滑跟随加速度计观测方向变化，不会出现跳变。第1步输出应接近0.2°左右（由初始0°与首次观测1.8°融合得到），后续逐步逼近真实角度。
• 噪声抑制：对比加速度计输入（如第3步1.5°与第4步2.6°的跳变），滤波输出变化幅度明显小于观测跳变，证明高频噪声被有效滤除。
• 无漂移累积：当陀螺仪角速度改变方向（第5步变为−1.0°/s）时，滤波输出能及时响应，不会因积分漂移而持续偏离。

判定标准：滤波输出序列单调性合理、无发散、且始终介于相邻加速度计观测值之间（或与其偏差不超过0.5°），即可判定卡尔曼滤波实现正确。若需进一步验证，可将 Q 调大（如0.01）观察输出更信任陀螺仪、响应更快但噪声增加；将 R 调大（如1.0）观察输出更平滑但滞后增加。