卡尔曼滤波代码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 initial_Q, float initial_R) {
    kf->x = initial_x;
    kf->P = initial_P;
    kf->Q = initial_Q;
    kf->R = initial_R;
}

// 更新卡尔曼滤波器状态
void 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; // 更新估计误差协方差
}

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

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

    // 使用卡尔曼滤波器进行滤波
    for (int i = 0; i < sizeof(measurements) / sizeof(measurements[0]); i++) {
        kalman_filter_update(&kf, measurements[i]);
        printf("Filtered value at step %d: %.2f\n", i+1, kf.x);
    }

    return 0;
}

这段代码演示了一个简单的一维卡尔曼滤波器在连续测量中的应用。你可以根据自己的需求进行相应的修改和扩展。请注意，这只是一个基本示例，实际应用中可能需要更复杂的实现。