请用C语言编写卡尔曼滤波算法

由于卡尔曼滤波算法涉及到矩阵运算，因此需要使用矩阵库，这里我们使用GNU Scientific Library (GSL)。

以下是一个简单的卡尔曼滤波器实现，用于估计一个物体的位置和速度。其中，我们假设物体沿直线运动，且只有一个传感器可以测量其位置。

#include <stdio.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_blas.h>
#include <gsl/gsl_linalg.h>

// 定义卡尔曼滤波器结构体
typedef struct {
    // 状态向量 [position, velocity]
    gsl_vector *x; 
    // 状态协方差矩阵
    gsl_matrix *P;
    // 系统噪声协方差矩阵
    gsl_matrix *Q;
    // 测量噪声协方差矩阵
    gsl_matrix *R;
    // 系统转移矩阵
    gsl_matrix *A;
    // 测量矩阵
    gsl_matrix *H;
} kalman_filter;

// 初始化卡尔曼滤波器
void kalman_filter_init(kalman_filter *kf, double position, double velocity, double dt, double sys_noise, double meas_noise)
{
    // 初始化状态向量
    kf->x = gsl_vector_alloc(2);
    gsl_vector_set(kf->x, 0, position);
    gsl_vector_set(kf->x, 1, velocity);

    // 初始化状态协方差矩阵
    kf->P = gsl_matrix_alloc(2, 2);
    gsl_matrix_set_identity(kf->P);

    // 初始化系统噪声协方差矩阵
    kf->Q = gsl_matrix_alloc(2, 2);
    gsl_matrix_set_zero(kf->Q);
    gsl_matrix_set(kf->Q, 0, 0, sys_noise);
    gsl_matrix_set(kf->Q, 1, 1, sys_noise);

    // 初始化测量噪声协方差矩阵
    kf->R = gsl_matrix_alloc(1, 1);
    gsl_matrix_set(kf->R, 0, 0, meas_noise);

    // 初始化系统转移矩阵
    kf->A = gsl_matrix_alloc(2, 2);
    gsl_matrix_set_identity(kf->A);
    gsl_matrix_set(kf->A, 0, 1, dt);

    // 初始化测量矩阵
    kf->H = gsl_matrix_alloc(1, 2);
    gsl_matrix_set_zero(kf->H);
    gsl_matrix_set(kf->H, 0, 0, 1);
}

// 更新卡尔曼滤波器
void kalman_filter_update(kalman_filter *kf, double measurement, double dt)
{
    // 预测
    gsl_vector *x_pred = gsl_vector_alloc(2);
    gsl_blas_dgemv(CblasNoTrans, 1.0, kf->A, kf->x, 0.0, x_pred);

    gsl_matrix *P_pred = gsl_matrix_alloc(2, 2);
    gsl_blas_dgemm(CblasNoTrans, CblasTrans, 1.0, kf->A, kf->P, 0.0, P_pred);
    gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, P_pred, kf->A, 0.0, P_pred);
    gsl_matrix_add(P_pred, kf->Q);

    // 更新
    gsl_vector *y = gsl_vector_alloc(1);
    gsl_vector_set(y, 0, measurement - gsl_matrix_get(kf->H, 0, 0) * gsl_vector_get(x_pred, 0));

    gsl_matrix *S = gsl_matrix_alloc(1, 1);
    gsl_blas_dgemm(CblasNoTrans, CblasTrans, 1.0, kf->H, P_pred, 0.0, S);
    gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, S, kf->H, 0.0, S);
    gsl_matrix_add(S, kf->R);

    gsl_matrix *K = gsl_matrix_alloc(2, 1);
    gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, P_pred, kf->H, 0.0, K);
    gsl_linalg_cholesky_decomp(S);
    gsl_linalg_cholesky_solve(S, y, K);

    gsl_vector *x_new = gsl_vector_alloc(2);
    gsl_vector_memcpy(x_new, x_pred);
    gsl_blas_dgemv(CblasNoTrans, 1.0, K, y, 1.0, x_new);

    gsl_matrix *P_new = gsl_matrix_alloc(2, 2);
    gsl_matrix_set_identity(P_new);
    gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, -1.0, K, kf->H, 1.0, P_new);

    // 更新滤波器状态
    gsl_vector_memcpy(kf->x, x_new);
    gsl_matrix_memcpy(kf->P, P_new);

    // 释放内存
    gsl_vector_free(x_pred);
    gsl_vector_free(y);
    gsl_vector_free(x_new);
    gsl_matrix_free(P_pred);
    gsl_matrix_free(S);
    gsl_matrix_free(K);
    gsl_matrix_free(P_new);
}

int main()
{
    // 初始化卡尔曼滤波器
    kalman_filter kf;
    kalman_filter_init(&kf, 0.0, 0.0, 0.1, 1.0, 0.1);

    // 模拟物体运动及测量
    double t = 0.0;
    double dt = 0.1;
    double pos = 0.0;
    double vel = 1.0;
    for (int i = 0; i < 100; i++) {
        // 更新物体状态
        pos += vel * dt;
        vel += 0.1 * dt;

        // 测量物体位置
        double meas = pos + 0.1 * (rand() / (double)RAND_MAX - 0.5);

        // 更新卡尔曼滤波器
        kalman_filter_update(&kf, meas, dt);

        // 输出滤波结果
        printf("%f,%f,%f\n", t, pos, gsl_vector_get(kf.x, 0));

        t += dt;
    }

    // 释放内存
    gsl_vector_free(kf.x);
    gsl_matrix_free(kf.P);
    gsl_matrix_free(kf.Q);
    gsl_matrix_free(kf.R);
    gsl_matrix_free(kf.A);
    gsl_matrix_free(kf.H);

    return 0;
}