卡尔曼滤波c代码

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

#include<stdio.h>

float kalman_filter(float z);
float Q = 0.0001; // 过程噪声协方差
float R = 0.001; // 测量噪声协方差
float x = 0, P = 1, K;

int main()
{
    float z[5] = { 1.2, 1.5, 1.3, 1.6, 1.7 };
    for (int i = 0; i < 5; i++)
        printf("Kalman Filter Output : %f\n", kalman_filter(z[i]));
    return 0;
}

float kalman_filter(float z)
{
    // 预测
    x = x;
    P = P + Q;

    // 更新
    K = P / (P + R);
    x = x + K * (z - x);
    P = (1 - K) * P;

    return x;
}

该代码演示了如何使用卡尔曼滤波来平滑输入信号。对于每个新的测量值，该算法使用先前的估计值和测量值来计算新的估计值。在此示例中，使用一个简单的输入信号来演示算法的效果。