112 lines
3.1 KiB
C
112 lines
3.1 KiB
C
/*
|
||
四元数扩展卡尔曼滤波器（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
|