单片机卡尔曼滤波c语言实现

以下是单片机卡尔曼滤波C语言实现的示例代码：

#include <stdio.h>

// 卡尔曼滤波器结构体
typedef struct {
    float q; // 过程噪声协方差
    float r; // 测量噪声协方差
    float x; // 状态变量
    float p; // 估计误差协方差
    float k; // 卡尔曼增益
} kalman_filter_t;

// 初始化卡尔曼滤波器
void kalman_filter_init(kalman_filter_t *kf, float q, float r, float p, float x) {
    kf->q = q;
    kf->r = r;
    kf->p = p;
    kf->x = x;
}

// 卡尔曼滤波
float kalman_filter(kalman_filter_t *kf, float z) {
    // 预测
    kf->x = kf->x;
    kf->p = kf->p + kf->q;

    // 更新
    kf->k = kf->p / (kf->p + kf->r);
    kf->x = kf->x + kf->k * (z - kf->x);
    kf->p = (1 - kf->k) * kf->p;

    return kf->x;
}

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

    // 模拟测量值
    float z[] = {1.2, 1.5, 1.3, 1.6, 1.4};

    // 卡尔曼滤波
    for (int i = 0; i < 5; i++) {
        float x = kalman_filter(&kf, z[i]);
        printf("z[%d] = %f, x[%d] = %f\n", i, z[i], i, x);
    }

    return 0;
}