c语言 卡尔曼滤波代码

这里提供一个简单的C语言实现卡尔曼滤波的代码示例：

#include <stdio.h>

// 定义卡尔曼滤波器的参数
float x_hat = 0;       // 状态估计值
float p = 1;           // 状态估计值的方差
float q = 0.01;        // 状态转移噪声的方差
float r = 0.1;         // 测量噪声的方差

// 定义测量值和真实值
float z = 0.5;         // 测量值
float x = 0.7;         // 真实值

// 卡尔曼滤波函数
void kalman_filter()
{
    // 预测
    float x_hat_minus = x_hat;
    float p_minus = p + q;

    // 更新
    float k = p_minus / (p_minus + r);
    x_hat = x_hat_minus + k * (z - x_hat_minus);
    p = (1 - k) * p_minus;
}

int main()
{
    // 模拟测量过程
    for(int i = 0; i < 100; i++)
    {
        z = x + 0.1 * i;    // 测量值随着时间逐渐增加
        kalman_filter();
        printf("%f\n", x_hat);  // 打印卡尔曼滤波后的状态估计值
    }

    return 0;
}

这个示例中，我们定义了卡尔曼滤波器的参数，并在`kalman_filter()`函数中实现了卡尔曼滤波的过程。在`main()`函数中，我们模拟了一个测量过程，并在每次测量后调用`kalman_filter()`函数进行滤波，最终输出卡尔曼滤波后的状态估计值。具体的实现细节可以根据具体的应用场景进行调整。