卡尔曼滤波算法 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_predict(KalmanFilter* kf) {
    kf->x = 0.0;  // 根据系统模型进行状态预测
    kf->P += kf->Q;  // 更新状态协方差
}

// 卡尔曼滤波器更新步骤
void kalman_filter_update(KalmanFilter* kf, float measurement) {
    float K = kf->P / (kf->P + kf->R);  // 计算卡尔曼增益
    kf->x += K * (measurement - kf->x);  // 更新状态估计值
    kf->P *= (1 - K);  // 更新状态协方差
}

int main() {
    KalmanFilter kf;
    float measurement = 10.0;  // 测量值

    // 初始化卡尔曼滤波器
    kalman_filter_init(&kf, 0.0, 1.0, 0.1, 0.1);

    // 进行卡尔曼滤波
    for (int i = 0; i < 10; i++) {
        kalman_filter_predict(&kf);
        kalman_filter_update(&kf, measurement);
        printf("Estimated state: %f\n", kf.x);
    }

    return 0;
}

这段代码实现了一个简单的一维卡尔曼滤波器，通过预测和更新步骤来估计系统的状态。你可以根据实际需求进行参数的调整和扩展。希望对你有帮助！