帮我写一个C语言卡尔曼滤波算法

当然可以，请看以下的 C 语言代码实现卡尔曼滤波算法：

#include <stdio.h>

#define Q 0.01  // 过程噪声协方差
#define R 0.1   // 测量噪声协方差

float kalman_filter(float z, float x, float p) {
    float xp = x;  // 估计值
    float pp = p + Q;  // 估计方差
    float k = pp / (pp + R);  // 卡尔曼增益
    float x1 = xp + k * (z - xp);  // 新的估计值
    float p1 = (1 - k) * pp;  // 新的估计方差
    return x1;
}

int main() {
    float z[] = {1.2, 1.4, 1.6, 1.8, 2.0};  // 测量值
    float x = 1.0;  // 估计值
    float p = 1.0;  // 估计方差
    int n = sizeof(z) / sizeof(z[0]);
    for(int i = 0; i < n; i++) {
        x = kalman_filter(z[i], x, p);
        p = (1 - 1/(i+1)) * p + 1/(i+1) * (x - z[i]) * (x - z[i]);
        printf("第 %d 次测量后的估计值：%.2f\n", i+1, x);
    }
    return 0;
}

卡尔曼滤波算法是一种用于估计系统状态的方法，其基本思想是通过将测量值与系统状态的预测值进行加权平均来获得更准确的估计值。在上述代码中，我们定义了一个 kalman_filter() 函数，它接受一个测量值 z、一个估计值 x 和一个估计方差 p 作为输入，并返回一个新的估计值。在主函数中，我们定义了一个测量值数组 z[]，并通过调用 kalman_filter() 函数来计算每次测量后的估计值。我们还根据最新的估计值和测量值来更新估计方差 p。最后，我们输出每次测量后的估计值。