用C语言写一段卡尔曼滤波代码，用于对二维状态估计

好的，以下是一段简单的卡尔曼滤波代码，用于对二维状态估计：

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

#define dt 0.01 // 时间步长
#define R 0.01 // 测量噪声协方差
#define Q 0.001 // 状态噪声协方差

int main()
{
    // 初始化
    double x[2] = {0}; // 初始状态
    double P[4] = {1, 0, 0, 1}; // 初始协方差矩阵
    double F[4] = {1, dt, 0, 1}; // 状态转移矩阵
    double H[2] = {1, 0}; // 观测矩阵
    double z[2]; // 观测量
    double K[2]; // 卡尔曼增益

    // 生成随机数据
    srand(0);
    double noise[2] = {sqrt(R) * rand(), sqrt(R) * rand()}; // 测量噪声

    // 更新状态估计
    for (int i = 0; i < 100; i++)
    {
        // 状态转移预测
        double x_hat[2] = {F[0] * x[0] + F[1] * x[1], F[2] * x[0] + F[3] * x[1]}; // 预测状态
        double P_hat[4] = {F[0] * P[0] + F[1] * P[2], F[0] * P[1] + F[1] * P[3],
                           F[2] * P[0] + F[3] * P[2], F[2] * P[1] + F[3] * P[3]}; // 预测协方差矩阵

        // 更新观测
        z[0] = x_hat[0] + noise[0]; // 观测量
        z[1] = x_hat[1] + noise[1];

        // 计算卡尔曼增益
        double S = H[0] * P_hat[0] + H[1] * P_hat[2] + R; // 观测噪声协方差
        K[0] = P_hat[0] * H[0] / S; // 卡尔曼增益
        K[1] = P_hat[2] * H[0] / S;

        // 更新状态估计
        x[0] = x_hat[0] + K[0] * (z[0] - H[0] * x_hat[0]); // 更新状态
        x[1] = x_hat[1] + K[1] * (z[1] - H[0] * x_hat[1]);
        P[0] = (1 - K[0] * H[0]) * P_hat[0] - K[0] * H[1] * P_hat[2]; // 更新协方差矩阵
        P[1] = (1 - K[0] * H[0]) * P_hat[1] - K[0] * H[1] * P_hat[3];
        P[2] = -K[1] * H[0] * P_hat[0] + (1 - K[1] * H[0]) * P_hat[2];
        P[3] = -K[1] * H[0] * P_hat[1] + (1 - K[1] * H[0]) * P_hat[3];

        printf("x1=%f, x2=%f\n", x[0], x[1]); // 输出状态
    }

    return 0;
}

该代码使用了卡尔曼滤波算法，对二维状态进行估计。其中，`x`表示状态向量，`P`表示协方差矩阵，`F`表示状态转移矩阵，`H`表示观测矩阵，`z`表示观测量，`K`表示卡尔曼增益。在每次循环中，首先进行状态转移预测，然后根据观测值更新状态估计和协方差矩阵。最后输出状态向量的值。