c语言实现kalman滤波

Kalman滤波是一种用于状态估计的方法，可以通过一些测量值来估计系统的状态。下面是一个简单的C语言实现Kalman滤波的示例代码：

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

#define PI 3.14159265

// Kalman filter variables
double Q = 0.00001;
double R = 0.01;
double x = 0, P = 1, K;

// Measured value
double measurement = 1.2;

int main()
{
    for(int i=0; i<20; i++)
    {
        // Time update
        x = x;
        P = P + Q;

        // Measurement update
        K = P / (P + R);
        x = x + K * (measurement - x);
        P = (1 - K) * P;

        printf("Measurement: %f\tKalman: %f\n", measurement, x);

        // Generate new measurement for next iteration
        measurement = sin(2*PI*(i+1)/10) + (double)rand()/RAND_MAX*0.1 - 0.05;
    }

    return 0;
}

在这个示例中，我们使用了一个简单的状态方程x = x，因为我们假设状态不随时间变化。我们还定义了一个过程噪声Q和一个测量噪声R。在每次迭代中，我们首先进行时间更新，然后进行测量更新。在测量更新中，我们计算卡尔曼增益K，并使用测量值和估计值来更新状态和协方差矩阵。最后，我们打印出测量值和卡尔曼滤波器的估计值。

注意，这只是一个简单的示例，Kalman滤波器的实现可以更加复杂和灵活，具体取决于应用场景和系统模型。