纯C语言如何使用卡尔曼滤波器对测量数据进行滤波。在函数kalman_init中，初始化卡尔曼滤波器结构体，其中包含过程噪声协方差Q，测量噪声协方差R，状态估计值x，和误差协方差p

纯C语言如何使用卡尔曼滤波器对测量数据进行滤波。在函数kalman_init中，初始化卡尔曼滤波器结构体，其中包含过程噪声协方差Q，测量噪声协方差R，状态估计值x，和误差协方差p

#include <stdio.h>

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

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

// 卡尔曼滤波函数
float kalman_filter(KalmanFilter *kf, float z) {
    // 预测
    float x_pre = kf->x;
    float p_pre = kf->p + kf->Q;
    // 更新
    kf->k = p_pre / (p_pre + kf->R);
    kf->x = x_pre + kf->k * (z - x_pre);
    kf->p = (1 - kf->k) * p_pre;
    return kf->x;
}

int main() {
    // 初始化卡尔曼滤波器
    KalmanFilter kf;
    kalman_init(&kf, 0.01, 0.1, 0, 1);
    // 模拟测量数据
    float z[] = {1.1, 1.2, 1.3, 1.4, 1.5, 1.6, 1.7, 1.8, 1.9, 2.0};
    // 使用卡尔曼滤波器进行滤波
    for (int i = 0; i < sizeof(z)/sizeof(float); i++) {
        float x = kalman_filter(&kf, z[i]);
        printf("%f\n", x);
    }
    return 0;
}

运行后输出效果如下：

 将数据导入至Excel后生成折线图如下：