帮我将这段代码转换为纯C代码；value_of_ukf ukfvectordouble X0 vectorvectordouble P0 double dt vectorpairdouble double contr_qh_cha

由于C语言无法使用STL库中的vector，因此需要手动实现向量和矩阵的操作。以下是可能的C代码实现：

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

#define N_STATE 4  // 状态向量的长度
#define N_MEAS 2   // 测量向量的长度
#define N_SIGMA 2*N_STATE+1  // sigma点的数量

typedef struct {
    double x[N_STATE];  // 状态向量
    double P[N_STATE][N_STATE];  // 状态协方差矩阵
} state_t;

typedef struct {
    double z[N_MEAS];  // 测量向量
    double R[N_MEAS][N_MEAS];  // 测量噪声矩阵
} meas_t;

typedef struct {
    double X[N_STATE][N_SIGMA];  // sigma点矩阵
    double W[N_SIGMA];  // 权重向量
    double Q[N_STATE][N_STATE];  // 状态噪声矩阵
} sigma_t;

typedef struct {
    double x[N_STATE];  // 估计的状态向量
    double P[N_STATE][N_STATE];  // 估计的状态协方差矩阵
} estimate_t;

// 向量加法
void vec_add(double *a, double *b, double *c, int n) {
    for (int i = 0; i < n; i++) {
        c[i] = a[i] + b[i];
    }
}

// 向量减法
void vec_sub(double *a, double *b, double *c, int n) {
    for (int i = 0; i < n; i++) {
        c[i] = a[i] - b[i];
    }
}

// 矩阵向量乘法
void mat_vec_mul(double (*A)[N_STATE], double *b, double *c, int m, int n) {
    for (int i = 0; i < m; i++) {
        double sum = 0;
        for (int j = 0; j < n; j++) {
            sum += A[i][j] * b[j];
        }
        c[i] = sum;
    }
}

// 矩阵加法
void mat_add(double (*A)[N_STATE], double (*B)[N_STATE], double (*C)[N_STATE], int m, int n) {
    for (int i = 0; i < m; i++) {
        for (int j = 0; j < n; j++) {
            C[i][j] = A[i][j] + B[i][j];
        }
    }
}

// 矩阵减法
void mat_sub(double (*A)[N_STATE], double (*B)[N_STATE], double (*C)[N_STATE], int m, int n) {
    for (int i = 0; i < m; i++) {
        for (int j = 0; j < n; j++) {
            C[i][j] = A[i][j] - B[i][j];
        }
    }
}

// 矩阵转置
void mat_transpose(double (*A)[N_STATE], double (*B)[N_STATE], int m, int n) {
    for (int i = 0; i < m; i++) {
        for (int j = 0; j < n; j++) {
            B[j][i] = A[i][j];
        }
    }
}

// 矩阵乘法
void mat_mul(double (*A)[N_STATE], double (*B)[N_STATE], double (*C)[N_STATE], int m, int n, int p) {
    for (int i = 0; i < m; i++) {
        for (int j = 0; j < p; j++) {
            double sum = 0;
            for (int k = 0; k < n; k++) {
                sum += A[i][k] * B[k][j];
            }
            C[i][j] = sum;
        }
    }
}

// 向量的点乘
double vec_dot(double *a, double *b, int n) {
    double sum = 0;
    for (int i = 0; i < n; i++) {
        sum += a[i] * b[i];
    }
    return sum;
}

// 矩阵的迹
double mat_trace(double (*A)[N_STATE], int n) {
    double sum = 0;
    for (int i = 0; i < n; i++) {
        sum += A[i][i];
    }
    return sum;
}

// sigma点生成函数
void sigma_points(state_t *X, sigma_t *S) {
    double lambda = 3 - N_STATE;
    double alpha = 1e-3;
    double beta = 2;
    double c = lambda + N_STATE;
    double k = alpha * alpha * (N_STATE + c) - N_STATE;
    double w0 = k / (N_STATE + k);
    double wi = 1 / (2 * (N_STATE + k));
    S->X[0][0] = X->x[0];
    S->X[1][0] = X->x[1];
    S->X[2][0] = X->x[2];
    S->X[3][0] = X->x[3];
    double offset = sqrt(N_STATE + k) * sqrt(X->P[0][0]);
    for (int i = 0; i < N_STATE; i++) {
        S->X[i][i+1] = X->x[i] + offset;
        S->X[i][i+1+N_STATE] = X->x[i] - offset;
    }
    for (int i = 0; i < N_SIGMA; i++) {
        S->W[i] = (i == 0) ? w0 : wi;
    }
}

// 非线性函数f
void f_func(double *x, double *u, double dt, double *y) {
    double v = u[1];
    double phi = x[2];
    double omega = u[0];
    double phi_new = phi + omega * dt;
    y[0] = x[0] + v / omega * (sin(phi_new) - sin(phi));
    y[1] = x[1] + v / omega * (-cos(phi_new) + cos(phi));
    y[2] = phi_new;
    y[3] = omega;
}

// 非线性函数h
void h_func(double *x, double *y) {
    y[0] = x[0];
    y[1] = x[1];
}

// UKF算法
estimate_t ukf(state_t *X, meas_t *Z, double dt, double qh, double cha) {
    sigma_t S;
    sigma_points(X, &S);
    double Q[N_STATE][N_STATE] = {
        {qh, 0, 0, 0},
        {0, qh, 0, 0},
        {0, 0, cha, 0},
        {0, 0, 0, cha}
    };
    S.Q[0][0] = Q[0][0];
    S.Q[1][1] = Q[1][1];
    S.Q[2][2] = Q[2][2];
    S.Q[3][3] = Q[3][3];
    double u[N_STATE] = {0};
    double y[N_STATE] = {0};
    for (int i = 0; i < N_SIGMA; i++) {
        f_func(S.X[i], u, dt, y);
        vec_add(y, S.X[i], S.X[i], N_STATE);
    }
    double x_pred[N_STATE] = {0};
    double P_pred[N_STATE][N_STATE] = {0};
    for (int i = 0; i < N_SIGMA; i++) {
        S.W[i] /= 2 * (N_STATE + S.Q[0][0] + S.Q[1][1] + S.Q[2][2] + S.Q[3][3]);
        vec_add(x_pred, S.X[i], x_pred, N_STATE);
    }
    vec_add(x_pred, X->x, x_pred, N_STATE);
    for (int i = 0; i < N_SIGMA; i++) {
        double x_diff[N_STATE] = {0};
        vec_sub(S.X[i], x_pred, x_diff, N_STATE);
        double P_temp[N_STATE][N_STATE] = {0};
        mat_mul(x_diff, x_diff, P_temp, 1, N_STATE, 1);
        mat_add(P_temp, S.Q, P_temp, N_STATE, N_STATE);
        for (int j = 0; j < N_STATE; j++) {
            for (int k = 0; k < N_STATE; k++) {
                P_pred[j][k] += S.W[i] * P_temp[j][k];
            }
        }
    }
    estimate_t est;
    vec_add(x_pred, Z->z, y, N_MEAS);
    est.x[0] = y[0];
    est.x[1] = y[1];
    mat_mul(P_pred, Z->R, P_pred, N_STATE, N_STATE, N_MEAS);
    mat_add(P_pred, X->P, est.P, N_STATE, N_STATE);
    return est;
}

int main() {
    state_t X = {
        .x = {0, 0, 0, 0},
        .P = {
            {1, 0, 0, 0},
            {0, 1, 0, 0},
            {0, 0, 1, 0},
            {0, 0, 0, 1}
        }
    };
    meas_t Z = {
        .z = {0, 0},
        .R = {
            {1, 0},
            {0, 1}
        }
    };
    double dt = 0.1;
    double qh = 0.01;
    double cha = 0.01;
    estimate_t est = ukf(&X, &Z, dt, qh, cha);
    printf("x: %lf, %lf, %lf, %lf\n", est.x[0], est.x[1], est.x[2], est.x[3]);
    printf("P:\n");
    for (int i = 0; i < N_STATE; i++) {
        for (int j = 0; j < N_STATE; j++) {
            printf("%lf ", est.P[i][j]);
        }
        printf("\n");
    }
    return 0;
}