卡尔曼滤波的C语言代码

卡尔曼滤波是一种用于估计系统状态的算法，它通过融合传感器测量值和系统模型来提供更准确的状态估计。以下是一个简单的卡尔曼滤波的C语言代码示例：

#include <stdio.h>

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

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

// 更新卡尔曼滤波器状态估计值和估计误差协方差
void kalman_filter_update(KalmanFilter* kf, float measurement) {
    // 预测步骤
    float x_pred = kf->x;
    float P_pred = kf->P + kf->Q;

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

int main() {
    // 创建卡尔曼滤波器对象
    KalmanFilter kf;
    // 初始化卡尔曼滤波器
    kalman_filter_init(&kf, 0, 1, 0.1, 0.1);

    // 模拟测量值
    float measurements[] = {1.2, 1.4, 1.6, 1.8, 2.0};

    // 使用卡尔曼滤波器进行状态估计
    for (int i = 0; i < sizeof(measurements) / sizeof(float); i++) {
        kalman_filter_update(&kf, measurements[i]);
        printf("Measurement: %.2f, Estimated State: %.2f\n", measurements[i], kf.x);
    }

    return 0;
}

这段代码实现了一个简单的一维卡尔曼滤波器，通过不断更新状态估计值和估计误差协方差来提供更准确的状态估计。你可以根据实际需求进行修改和扩展。