卡曼滤波代码C语言代码

卡曼滤波是一种常用的状态估计方法，可以用于降噪、预测和校正等应用。以下是一个简单的卡曼滤波的C语言实现示例：

#include <stdio.h>
// 卡曼滤波参数
float Q = 0.1;   // 过程噪声协方差
float R = 1.0;   // 观测噪声协方差
float x_hat = 0; // 状态估计量
float p = 1;     // 估计误差协方差

// 卡曼滤波函数
float kalman_filter(float z)
{
    // 预测步骤
    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;

    return x_hat;
}

int main()
{
    float measurements[] = {1.0, 2.0, 3.0, 4.0, 5.0}; // 测量值数组

    int num_measurements = sizeof(measurements) / sizeof(measurements[0]);

    for (int i = 0; i < num_measurements; ++i)
    {
        float filtered_value = kalman_filter(measurements[i]);
        printf("Measurement: %.2f, Filtered value: %.2f\n", measurements[i], filtered_value);
    }

    return 0;
}

这段代码实现了一个简单的一维卡曼滤波器。通过不断输入测量值 `measurements`，可以得到相应的滤波结果。该代码中的卡曼滤波参数可以根据实际情况进行调整。