卡尔曼滤波算法c语言 代码

以下是一个简单的卡尔曼滤波器的C语言实现代码。

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

//卡尔曼滤波器参数
double Q = 0.01; //过程噪声方差
double R = 0.1; //测量噪声方差
double x = 0; //状态估计值
double p = 1; //状态估计方差

double kalman_filter(double z)
{
    double kg = p / (p + R); //卡尔曼增益
    x = x + kg * (z - x); //更新状态估计值
    p = (1 - kg) * p + Q; //更新状态估计方差
    return x; //返回估计值
}

int main()
{
    double z[] = {1, 1.5, 2, 3, 4}; //测量值
    int n = sizeof(z) / sizeof(double); //测量值数量

    printf("Kalman Filter:\n");
    for (int i = 0; i < n; i++)
    {
        double y = kalman_filter(z[i]); //卡尔曼滤波
        printf("z[%d] = %.2f, y[%d] = %.2f\n", i, z[i], i, y);
    }

    return 0;
}

这个代码实现了一个简单的一维卡尔曼滤波器，可以通过修改Q和R参数来调整卡尔曼滤波器的性能。在实际应用中，需要根据具体情况调整参数。