c语言卡尔曼滤波代码

下面是一个简单的C语言卡尔曼滤波代码示例，假设我们要对一个简单的一维系统进行状态估计。

#include <stdio.h>

/* 系统模型参数 */
double A = 1.0;  // 状态转移矩阵
double H = 1.0;  // 观测矩阵
double Q = 0.1;  // 状态噪声协方差
double R = 1.0;  // 观测噪声协方差

/* 初始状态 */
double x_hat = 0.0;  // 初始状态估计值
double P = 1.0;      // 初始状态协方差矩阵

/* 状态更新 */
void update(double z)
{
    // 预测步骤
    double x_hat_minus = A * x_hat;
    double P_minus = A * P * A + Q;

    // 更新步骤
    double K = P_minus * H / (H * P_minus * H + R);
    x_hat = x_hat_minus + K * (z - H * x_hat_minus);
    P = (1 - K * H) * P_minus;

    printf("x_hat = %f\n", x_hat);
}

int main()
{
    update(1.0);  // 观测值为1.0
    update(2.0);  // 观测值为2.0
    update(3.0);  // 观测值为3.0

    return 0;
}

在上面的代码中，`update`函数对应卡尔曼滤波算法的一次迭代。首先进行预测步骤，根据上一时刻的状态估计值和协方差矩阵，通过状态转移矩阵进行预测。然后进行更新步骤，根据观测值和观测矩阵计算卡尔曼增益，更新状态估计值和协方差矩阵。最后输出当前的状态估计值。在`main`函数中，我们可以对不同的观测值进行多次迭代，不断更新状态估计值。