C语言实现卡尔曼滤波算法

以下是一个简单的C语言实现卡尔曼滤波算法的示例代码：

#include <stdio.h>

// 定义卡尔曼滤波器结构体
typedef struct {
    float x;  // 状态估计值
    float P;  // 估计误差协方差
    float Q;  // 系统噪声协方差
    float R;  // 测量噪声协方差
    float K;  // 卡尔曼增益
} KalmanFilter;

// 初始化卡尔曼滤波器
void kalman_filter_init(KalmanFilter* kf, float initial_x, float initial_P, float Q, float R) {
    kf->x = initial_x;
    kf->P = initial_P;
    kf->Q = Q;
    kf->R = R;
}

// 更新卡尔曼滤波器状态
float kalman_filter_update(KalmanFilter* kf, float measurement) {
    // 预测步骤
    float x_pred = kf->x;
    float P_pred = kf->P + kf->Q;

    // 更新步骤
    kf->K = P_pred / (P_pred + kf->R);
    kf->x = x_pred + kf->K * (measurement - x_pred);
    kf->P = (1 - kf->K) * P_pred;

    return kf->x;
}

int main() {
    KalmanFilter kf;
    float measurement[] = {1.2, 1.8, 2.4, 3.3, 3.9};  // 测量值

    kalman_filter_init(&kf, 0, 1, 0.01, 0.1);  // 初始化卡尔曼滤波器

    printf("Kalman Filter Output:\n");
    for (int i = 0; i < sizeof(measurement) / sizeof(float); i++) {
        float filtered_value = kalman_filter_update(&kf, measurement[i]);
        printf("%.2f ", filtered_value);
    }
    printf("\n");

    return 0;
}

在这个示例代码中，我们定义了一个KalmanFilter结构体来保存卡尔曼滤波器的状态。通过调用`kalman_filter_init`函数来初始化滤波器，然后使用`kalman_filter_update`函数来更新滤波器状态并输出滤波后的值。

注意，这只是一个简单的示例，实际应用中可能需要根据具体问题来调整参数和数据处理部分。