帮我写一个C语言的卡尔曼滤波函数

好的，以下是一个简单的C语言卡尔曼滤波函数的代码：

void kalman_filter(float z, float *x, float *p)
{
    float q = 0.1; // 过程噪声协方差
    float r = 0.5; // 测量噪声协方差
    float k; // 卡尔曼增益
    float x0 = *x; // 上一时刻状态估计值
    float p0 = *p; // 上一时刻状态估计协方差

    // 预测
    float x1 = x0;
    float p1 = p0 + q;

    // 更新
    k = p1 / (p1 + r);
    x1 = x1 + k * (z - x1);
    p1 = (1 - k) * p1;

    // 更新状态估计值和协方差
    *x = x1;
    *p = p1;
}

这个函数接受三个参数：测量值z、状态估计值x和状态估计协方差p。函数会根据卡尔曼滤波算法对状态进行估计和更新，最后返回更新后的状态估计值和协方差。