无迹卡尔曼滤波算法（C语言代码）

无迹卡尔曼滤波（Unscented Kalman Filter, UKF）是一种非线性状态估计算法，它通过无迹变换来处理非线性系统，相比扩展卡尔曼滤波（EKF），UKF在处理非线性系统时更具鲁棒性。下面是一个简单的无迹卡尔曼滤波器的C语言实现示例。这个实现展示了如何定义UKF并进行状态估计。

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

// 定义矩阵和向量的大小
#define STATE_DIM 4
#define MEAS_DIM 2

// 无迹卡尔曼滤波器结构体
typedef struct {
    float X[STATE_DIM]; // 状态向量
    float P[STATE_DIM * STATE_DIM]; // 状态协方差矩阵
    float Q[STATE_DIM * STATE_DIM]; // 过程噪声协方差
    float R[MEAS_DIM * MEAS_DIM]; // 测量噪声协方差
    float H[MEAS_DIM * STATE_DIM]; // 测量矩阵
    float K[STATE_DIM * MEAS_DIM]; // 卡尔曼增益
    float X_pred[STATE_DIM]; // 预测状态向量
    float P_pred[STATE_DIM * STATE_DIM]; // 预测协方差矩阵
} UKF;

// 矩阵相乘函数
void matrix_mult(float* A, float* B, float* C, int rows_A, int cols_A, int cols_B) {
    for (int i = 0; i < rows_A; i++) {
        for (int j = 0; j < cols_B; j++) {
            C[i * cols_B + j] =