/*
  四元数扩展卡尔曼滤波器（QEKF）
  基于陀螺仪零偏估计和卡方检验的姿态更新算法
*/

#pragma once

#ifdef __cplusplus
extern "C" {
#endif

#include "kalman_filter.h"

/* USER INCLUDE BEGIN */

/* USER INCLUDE END */

/* USER DEFINE BEGIN */

/* USER DEFINE END */

/* 布尔类型定义 */
#ifndef TRUE
#define TRUE 1
#endif

#ifndef FALSE
#define FALSE 0
#endif

/* 四元数EKF惯性导航结构体 */
typedef struct {
  uint8_t initialized;      /* 初始化标志 */
  KF_t qekf;                /* 卡尔曼滤波器实例 */
  uint8_t converge_flag;    /* 收敛标志 */
  uint8_t stable_flag;      /* 稳定标志 */
  uint64_t error_count;     /* 错误计数 */
  uint64_t update_count;    /* 更新计数 */

  float q[4];               /* 四元数估计值 */
  float gyro_bias[3];       /* 陀螺仪零偏估计值 */

  float gyro[3];            /* 陀螺仪数据 */
  float accel[3];           /* 加速度计数据 */

  float orientation_cosine[3]; /* 方向余弦 */

  float acc_lpf_coef;       /* 加速度低通滤波系数 */
  float gyro_norm;          /* 陀螺仪模值 */
  float accl_norm;          /* 加速度计模值 */
  float adaptive_gain_scale; /* 自适应增益缩放 */

  float roll;               /* 横滚角 */
  float pitch;              /* 俯仰角 */
  float yaw;                /* 偏航角 */

  float yaw_total_angle;    /* 偏航总角度 */

  float Q1;                 /* 四元数更新过程噪声 */
  float Q2;                 /* 陀螺仪零偏过程噪声 */
  float R;                  /* 加速度计量测噪声 */

  float dt;                 /* 姿态更新周期 */
  KF_Mat chi_square;        /* 卡方检验矩阵 */
  float chi_square_data[1]; /* 卡方检验数据 */
  float chi_square_threshold; /* 卡方检验阈值 */
  float lambda;             /* 渐消因子 */

  int16_t yaw_round_count;  /* 偏航圈数计数 */
  float yaw_angle_last;     /* 上次偏航角 */
} QEKF_INS_t;

/* USER STRUCT BEGIN */

/* USER STRUCT END */

extern QEKF_INS_t QEKF_INS;

/**
 * @brief 四元数EKF初始化
 *
 * @param process_noise1 四元数过程噪声 (推荐值: 10)
 * @param process_noise2 陀螺仪零偏过程噪声 (推荐值: 0.001)
 * @param measure_noise 加速度计量测噪声 (推荐值: 1000000)
 * @param lambda 渐消因子 (推荐值: 0.9996)
 * @param lpf 低通滤波系数 (推荐值: 0)
 */
void QEKF_Init(float process_noise1, float process_noise2, float measure_noise,
               float lambda, float lpf);

/**
 * @brief 四元数EKF更新
 *
 * @param gx 陀螺仪X轴角速度 (rad/s)
 * @param gy 陀螺仪Y轴角速度 (rad/s)
 * @param gz 陀螺仪Z轴角速度 (rad/s)
 * @param ax 加速度计X轴加速度 (m/s²)
 * @param ay 加速度计Y轴加速度 (m/s²)
 * @param az 加速度计Z轴加速度 (m/s²)
 * @param dt 更新周期 (s)
 */
void QEKF_Update(float gx, float gy, float gz, float ax, float ay, float az,
                 float dt);

/* USER FUNCTION BEGIN */

/* USER FUNCTION END */

#ifdef __cplusplus
}
#endif