写一个 c语言 卡尔曼滤波函数

下面是一个简单的C语言卡尔曼滤波函数的示例代码：

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

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

typedef struct {
    float x; // 状态向量
    float p; // 状态协方差
    float k; // 卡尔曼增益
} kalman_state;

void kalman_filter(float z, kalman_state* state) {
    // 预测状态
    float x_pred = state->x;
    float p_pred = state->p + Q;

    // 更新状态
    state->k = p_pred / (p_pred + R);
    state->x = x_pred + state->k * (z - x_pred);
    state->p = (1 - state->k) * p_pred;
}

int main() {
    // 初始化状态
    kalman_state state = {0, 1, 0};

    // 模拟测量值
    float z = 0;
    for (int i = 0; i < 100; i++) {
        z = 0.5 * sin(i * 0.1) + 0.1 * rand() / RAND_MAX;
        kalman_filter(z, &state);
        printf("%f\n", state.x);
    }
    return 0;
}

这个示例代码实现了一个简单的一维卡尔曼滤波器，用于预测一个正弦波加上一些随机噪声的值。该滤波器使用了一个状态向量 x，一个状态协方差 p，和一个卡尔曼增益 k。在主函数中，我们通过模拟测量值来测试滤波器，并输出预测值。