#ifndef KALMAN_FILTER_H
#define KALMAN_FILTER_H

// 卡尔曼滤波器结构体
typedef struct {
    float x[4];           // 状态向量 [x, vx, y, vy]
    float P[4][4];        // 状态协方差矩阵
    float F[4][4];        // 状态转移矩阵
    float Q[4][4];        // 过程噪声协方差
    float R[2][2];        // 测量噪声协方差
    float dt;             // 采样时间间隔(秒)
} KalmanFilter;

// 函数声明
void kalman_init(KalmanFilter* kf, float dt, float initial_x, float initial_y);
void kalman_predict(KalmanFilter* kf);
void kalman_update(KalmanFilter* kf, float z_x, float z_y);
void kalman_get_position(KalmanFilter* kf, float* x, float* y);

#endif // KALMAN_FILTER_H