用C语言写一段卡尔曼滤波代码

下面给出一段基于C语言的卡尔曼滤波代码，用于对一维状态进行估计：

#include <stdio.h>
#include <stdlib.h>
#include <math.h>

#define Q 0.001 // 过程噪声方差
#define R 0.01 // 观测噪声方差

float x_hat = 0; // 状态估计值
float p = 1; // 估计误差方差
float k; // 卡尔曼增益

int main() {
    float z; // 观测值
    float u = 0; // 控制输入
    float a = 1; // 状态转移矩阵
    float b = 0; // 控制矩阵
    float h = 1; // 观测矩阵
    float q = Q; // 过程噪声方差
    float r = R; // 观测噪声方差
    
    // 初始化
    x_hat = 0;
    p = 1;

    // 循环
    for (int i = 0; i < 100; i++) {
        // 观测值
        z = sinf(i * M_PI / 10) + (float)rand() / RAND_MAX * sqrt(r);

        // 预测
        x_hat = a * x_hat + b * u;
        p = a * p * a + q;

        // 更新
        k = p * h / (h * p * h + r);
        x_hat = x_hat + k * (z - h * x_hat);
        p = (1 - k * h) * p;

        printf("%f\n", x_hat);
    }

    return 0;
}

该代码中，Q 和 R 分别表示过程噪声方差和观测噪声方差，x_hat 和 p 初始值分别为0和1，a、b、h分别为状态转移矩阵、控制矩阵和观测矩阵，q 和 r 分别为过程噪声方差和观测噪声方差。在循环中，首先根据观测值更新估计值和估计误差方差，然后进行预测和更新操作，最后输出估计值。

需要注意的是，在实际应用中，需要根据具体的系统模型和测量数据进行参数的调节和优化，以达到更好的估计效果。