robofish/rm_balance
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

添加ekf

Browse Source
This commit is contained in:
Robofish 2026-02-02 02:58:48 +08:00
parent be86c08ef4
commit e914c4647c
3 changed files with 775 additions and 137 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

527
User/component/QuaternionEKF.c Normal file
View File

@ -0,0 +1,527 @@
/*
四元数扩展卡尔曼滤波器（QEKF）
基于陀螺仪零偏估计和卡方检验的姿态更新算法
一阶低通滤波器传递函数:
1
———————
as + 1
*/
#include "QuaternionEKF.h"
#include <string.h>
/* USER INCLUDE BEGIN */
/* USER INCLUDE END */
/* USER DEFINE BEGIN */
/* USER DEFINE END */
/* 全局QEKF实例 */
QEKF_INS_t QEKF_INS;
/* 状态转移矩阵F初始值 */
static const float QEKF_F_Init[36] = {
1, 0, 0, 0, 0, 0,
0, 1, 0, 0, 0, 0,
0, 0, 1, 0, 0, 0,
0, 0, 0, 1, 0, 0,
0, 0, 0, 0, 1, 0,
0, 0, 0, 0, 0, 1
};
/* 协方差矩阵P初始值 */
static float QEKF_P_Init[36] = {
1, 0.1, 0.1, 0.1, 0.1, 0.1,
0.1, 1, 0.1, 0.1, 0.1, 0.1,
0.1, 0.1, 1, 0.1, 0.1, 0.1,
0.1, 0.1, 0.1, 1, 0.1, 0.1,
0.1, 0.1, 0.1, 0.1, 1, 0.1,
0.1, 0.1, 0.1, 0.1, 0.1, 1
};
/* 用于调试的矩阵副本 */
static float QEKF_K[18];
static float QEKF_H[18];
/* 私有函数声明 */
static float QEKF_InvSqrt(float x);
static void QEKF_Observe(KF_t *kf);
static void QEKF_F_Linearization_P_Fading(KF_t *kf);
static void QEKF_SetH(KF_t *kf);
static void QEKF_xhatUpdate(KF_t *kf);
/**
* @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) {
QEKF_INS.initialized = 1;
QEKF_INS.Q1 = process_noise1;
QEKF_INS.Q2 = process_noise2;
QEKF_INS.R = measure_noise;
QEKF_INS.chi_square_threshold = 1e-8f;
QEKF_INS.converge_flag = 0;
QEKF_INS.error_count = 0;
QEKF_INS.update_count = 0;
if (lambda > 1) {
lambda = 1;
}
QEKF_INS.lambda = lambda;
QEKF_INS.acc_lpf_coef = lpf;
/* 初始化卡尔曼滤波器（状态维度6，控制维度0，量测维度3） */
KF_Init(&QEKF_INS.qekf, 6, 0, 3);
KF_MatInit(&QEKF_INS.chi_square, 1, 1, QEKF_INS.chi_square_data);
/* 四元数初始化为单位四元数 */
QEKF_INS.qekf.xhat_data[0] = 1;
QEKF_INS.qekf.xhat_data[1] = 0;
QEKF_INS.qekf.xhat_data[2] = 0;
QEKF_INS.qekf.xhat_data[3] = 0;
/* 自定义函数初始化，用于扩展或增加KF的基础功能 */
QEKF_INS.qekf.User_Func0_f = QEKF_Observe;
QEKF_INS.qekf.User_Func1_f = QEKF_F_Linearization_P_Fading;
QEKF_INS.qekf.User_Func2_f = QEKF_SetH;
QEKF_INS.qekf.User_Func3_f = QEKF_xhatUpdate;
/* 设定标志位，用自定义函数替换KF标准步骤中的计算增益和后验估计 */
QEKF_INS.qekf.skip_eq3 = TRUE;
QEKF_INS.qekf.skip_eq4 = TRUE;
memcpy(QEKF_INS.qekf.F_data, QEKF_F_Init, sizeof(QEKF_F_Init));
memcpy(QEKF_INS.qekf.P_data, QEKF_P_Init, sizeof(QEKF_P_Init));
}
/**
* @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) {
/* 0.5*(Ohm-Ohm^bias)*deltaT，用于更新状态转移F矩阵 */
float halfgxdt, halfgydt, halfgzdt;
float accel_inv_norm;
if (!QEKF_INS.initialized) {
QEKF_Init(10, 0.001f, 1000000 * 10, 0.9996f * 0 + 1, 0);
}
/* F矩阵，带*号的位置需要设置
0 1* 2* 3* 4 5
6* 7 8* 9* 10 11
12* 13* 14 15* 16 17
18* 19* 20* 21 22 23
24 25 26 27 28 29
30 31 32 33 34 35
*/
QEKF_INS.dt = dt;
/* 去除零偏后的陀螺仪数据 */
QEKF_INS.gyro[0] = gx - QEKF_INS.gyro_bias[0];
QEKF_INS.gyro[1] = gy - QEKF_INS.gyro_bias[1];
QEKF_INS.gyro[2] = gz - QEKF_INS.gyro_bias[2];
/* 设置F矩阵 */
halfgxdt = 0.5f * QEKF_INS.gyro[0] * dt;
halfgydt = 0.5f * QEKF_INS.gyro[1] * dt;
halfgzdt = 0.5f * QEKF_INS.gyro[2] * dt;
/* 设定状态转移矩阵F的左上角4x4子矩阵，即0.5*(Ohm-Ohm^bias)*deltaT
右下角有一个2x2单位阵已经初始化好了
注意在predict步F的右上角是4x2的零矩阵，因此每次predict都用单位阵覆盖前一轮线性化后的矩阵 */
memcpy(QEKF_INS.qekf.F_data, QEKF_F_Init, sizeof(QEKF_F_Init));
QEKF_INS.qekf.F_data[1] = -halfgxdt;
QEKF_INS.qekf.F_data[2] = -halfgydt;
QEKF_INS.qekf.F_data[3] = -halfgzdt;
QEKF_INS.qekf.F_data[6] = halfgxdt;
QEKF_INS.qekf.F_data[8] = halfgzdt;
QEKF_INS.qekf.F_data[9] = -halfgydt;
QEKF_INS.qekf.F_data[12] = halfgydt;
QEKF_INS.qekf.F_data[13] = -halfgzdt;
QEKF_INS.qekf.F_data[15] = halfgxdt;
QEKF_INS.qekf.F_data[18] = halfgzdt;
QEKF_INS.qekf.F_data[19] = halfgydt;
QEKF_INS.qekf.F_data[20] = -halfgxdt;
/* 加速度低通滤波，平滑数据，降低撞击和异常的影响 */
if (QEKF_INS.update_count == 0) {
/* 第一次进入，初始化低通滤波 */
QEKF_INS.accel[0] = ax;
QEKF_INS.accel[1] = ay;
QEKF_INS.accel[2] = az;
}
QEKF_INS.accel[0] = QEKF_INS.accel[0] * QEKF_INS.acc_lpf_coef /
(QEKF_INS.dt + QEKF_INS.acc_lpf_coef) +
ax * QEKF_INS.dt / (QEKF_INS.dt + QEKF_INS.acc_lpf_coef);
QEKF_INS.accel[1] = QEKF_INS.accel[1] * QEKF_INS.acc_lpf_coef /
(QEKF_INS.dt + QEKF_INS.acc_lpf_coef) +
ay * QEKF_INS.dt / (QEKF_INS.dt + QEKF_INS.acc_lpf_coef);
QEKF_INS.accel[2] = QEKF_INS.accel[2] * QEKF_INS.acc_lpf_coef /
(QEKF_INS.dt + QEKF_INS.acc_lpf_coef) +
az * QEKF_INS.dt / (QEKF_INS.dt + QEKF_INS.acc_lpf_coef);
/* 设置量测向量z，单位化重力加速度向量 */
accel_inv_norm =
QEKF_InvSqrt(QEKF_INS.accel[0] * QEKF_INS.accel[0] +
QEKF_INS.accel[1] * QEKF_INS.accel[1] +
QEKF_INS.accel[2] * QEKF_INS.accel[2]);
for (uint8_t i = 0; i < 3; i++) {
QEKF_INS.qekf.measured_vector[i] = QEKF_INS.accel[i] * accel_inv_norm;
}
/* 获取机体状态 */
QEKF_INS.gyro_norm =
1.0f / QEKF_InvSqrt(QEKF_INS.gyro[0] * QEKF_INS.gyro[0] +
QEKF_INS.gyro[1] * QEKF_INS.gyro[1] +
QEKF_INS.gyro[2] * QEKF_INS.gyro[2]);
QEKF_INS.accl_norm = 1.0f / accel_inv_norm;
/* 如果角速度小于阈值且加速度处于设定范围内，认为运动稳定，加速度可以用于修正角速度 */
if (QEKF_INS.gyro_norm < 0.3f && QEKF_INS.accl_norm > 9.8f - 0.5f &&
QEKF_INS.accl_norm < 9.8f + 0.5f) {
QEKF_INS.stable_flag = 1;
} else {
QEKF_INS.stable_flag = 0;
}
/* 设置过程噪声Q和量测噪声R矩阵 */
QEKF_INS.qekf.Q_data[0] = QEKF_INS.Q1 * QEKF_INS.dt;
QEKF_INS.qekf.Q_data[7] = QEKF_INS.Q1 * QEKF_INS.dt;
QEKF_INS.qekf.Q_data[14] = QEKF_INS.Q1 * QEKF_INS.dt;
QEKF_INS.qekf.Q_data[21] = QEKF_INS.Q1 * QEKF_INS.dt;
QEKF_INS.qekf.Q_data[28] = QEKF_INS.Q2 * QEKF_INS.dt;
QEKF_INS.qekf.Q_data[35] = QEKF_INS.Q2 * QEKF_INS.dt;
QEKF_INS.qekf.R_data[0] = QEKF_INS.R;
QEKF_INS.qekf.R_data[4] = QEKF_INS.R;
QEKF_INS.qekf.R_data[8] = QEKF_INS.R;
/* 调用卡尔曼滤波更新，注意几个User_Funcx_f的调用 */
KF_Update(&QEKF_INS.qekf);
/* 获取融合后的数据，包括四元数和xy零偏值 */
QEKF_INS.q[0] = QEKF_INS.qekf.filtered_value[0];
QEKF_INS.q[1] = QEKF_INS.qekf.filtered_value[1];
QEKF_INS.q[2] = QEKF_INS.qekf.filtered_value[2];
QEKF_INS.q[3] = QEKF_INS.qekf.filtered_value[3];
QEKF_INS.gyro_bias[0] = QEKF_INS.qekf.filtered_value[4];
QEKF_INS.gyro_bias[1] = QEKF_INS.qekf.filtered_value[5];
QEKF_INS.gyro_bias[2] = 0; /* 大部分时候z轴通天，无法观测yaw的漂移 */
/* 利用四元数反解欧拉角 */
QEKF_INS.yaw =
atan2f(2.0f * (QEKF_INS.q[0] * QEKF_INS.q[3] +
QEKF_INS.q[1] * QEKF_INS.q[2]),
2.0f * (QEKF_INS.q[0] * QEKF_INS.q[0] +
QEKF_INS.q[1] * QEKF_INS.q[1]) - 1.0f) * 57.295779513f;
QEKF_INS.pitch =
atan2f(2.0f * (QEKF_INS.q[0] * QEKF_INS.q[1] +
QEKF_INS.q[2] * QEKF_INS.q[3]),
2.0f * (QEKF_INS.q[0] * QEKF_INS.q[0] +
QEKF_INS.q[3] * QEKF_INS.q[3]) - 1.0f) * 57.295779513f;
QEKF_INS.roll =
asinf(-2.0f * (QEKF_INS.q[1] * QEKF_INS.q[3] -
QEKF_INS.q[0] * QEKF_INS.q[2])) * 57.295779513f;
/* 获取yaw总角度，处理超过360度的情况（如小陀螺） */
if (QEKF_INS.yaw - QEKF_INS.yaw_angle_last > 180.0f) {
QEKF_INS.yaw_round_count--;
} else if (QEKF_INS.yaw - QEKF_INS.yaw_angle_last < -180.0f) {
QEKF_INS.yaw_round_count++;
}
QEKF_INS.yaw_total_angle = 360.0f * QEKF_INS.yaw_round_count + QEKF_INS.yaw;
QEKF_INS.yaw_angle_last = QEKF_INS.yaw;
QEKF_INS.update_count++;
}
/**
* @brief 更新线性化后的状态转移矩阵F右上角的4x2分块矩阵，用于协方差矩阵P的更新
* 并对零偏的方差进行限制，防止过度收敛并限幅防止发散
*
* @param kf 卡尔曼滤波器结构体指针
*/
static void QEKF_F_Linearization_P_Fading(KF_t *kf) {
float q0, q1, q2, q3;
float q_inv_norm;
q0 = kf->xhatminus_data[0];
q1 = kf->xhatminus_data[1];
q2 = kf->xhatminus_data[2];
q3 = kf->xhatminus_data[3];
/* 四元数归一化 */
q_inv_norm = QEKF_InvSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
for (uint8_t i = 0; i < 4; i++) {
kf->xhatminus_data[i] *= q_inv_norm;
}
/* F矩阵，带*号的位置需要设置
0 1 2 3 4* 5*
6 7 8 9 10* 11*
12 13 14 15 16* 17*
18 19 20 21 22* 23*
24 25 26 27 28 29
30 31 32 33 34 35
*/
/* 设置F矩阵右上角4x2分块 */
kf->F_data[4] = q1 * QEKF_INS.dt / 2;
kf->F_data[5] = q2 * QEKF_INS.dt / 2;
kf->F_data[10] = -q0 * QEKF_INS.dt / 2;
kf->F_data[11] = q3 * QEKF_INS.dt / 2;
kf->F_data[16] = -q3 * QEKF_INS.dt / 2;
kf->F_data[17] = -q0 * QEKF_INS.dt / 2;
kf->F_data[22] = q2 * QEKF_INS.dt / 2;
kf->F_data[23] = -q1 * QEKF_INS.dt / 2;
/* 渐消滤波，防止零偏参数过度收敛 */
kf->P_data[28] /= QEKF_INS.lambda;
kf->P_data[35] /= QEKF_INS.lambda;
/* 限幅，防止发散 */
if (kf->P_data[28] > 10000) {
kf->P_data[28] = 10000;
}
if (kf->P_data[35] > 10000) {
kf->P_data[35] = 10000;
}
}
/**
* @brief 在工作点处计算观测函数h(x)的Jacobi矩阵H
*
* @param kf 卡尔曼滤波器结构体指针
*/
static void QEKF_SetH(KF_t *kf) {
float doubleq0, doubleq1, doubleq2, doubleq3;
/* H矩阵布局
0 1 2 3 4 5
6 7 8 9 10 11
12 13 14 15 16 17
最后两列是零
*/
doubleq0 = 2 * kf->xhatminus_data[0];
doubleq1 = 2 * kf->xhatminus_data[1];
doubleq2 = 2 * kf->xhatminus_data[2];
doubleq3 = 2 * kf->xhatminus_data[3];
memset(kf->H_data, 0, sizeof(float) * kf->z_size * kf->xhat_size);
kf->H_data[0] = -doubleq2;
kf->H_data[1] = doubleq3;
kf->H_data[2] = -doubleq0;
kf->H_data[3] = doubleq1;
kf->H_data[6] = doubleq1;
kf->H_data[7] = doubleq0;
kf->H_data[8] = doubleq3;
kf->H_data[9] = doubleq2;
kf->H_data[12] = doubleq0;
kf->H_data[13] = -doubleq1;
kf->H_data[14] = -doubleq2;
kf->H_data[15] = doubleq3;
}
/**
* @brief 利用观测值和先验估计得到最优的后验估计
* 加入了卡方检验以判断融合加速度的条件是否满足
* 同时引入发散保护保证恶劣工况下的必要量测更新
*
* @param kf 卡尔曼滤波器结构体指针
*/
static void QEKF_xhatUpdate(KF_t *kf) {
float q0, q1, q2, q3;
kf->mat_status = KF_MatTrans(&kf->H, &kf->HT);
kf->temp_matrix.numRows = kf->H.numRows;
kf->temp_matrix.numCols = kf->Pminus.numCols;
/* temp_matrix = H·P'(k) */
kf->mat_status = KF_MatMult(&kf->H, &kf->Pminus, &kf->temp_matrix);
kf->temp_matrix1.numRows = kf->temp_matrix.numRows;
kf->temp_matrix1.numCols = kf->HT.numCols;
/* temp_matrix1 = H·P'(k)·HT */
kf->mat_status = KF_MatMult(&kf->temp_matrix, &kf->HT, &kf->temp_matrix1);
kf->S.numRows = kf->R.numRows;
kf->S.numCols = kf->R.numCols;
/* S = H·P'(k)·HT + R */
kf->mat_status = KF_MatAdd(&kf->temp_matrix1, &kf->R, &kf->S);
/* temp_matrix1 = inv(H·P'(k)·HT + R) */
kf->mat_status = KF_MatInv(&kf->S, &kf->temp_matrix1);
q0 = kf->xhatminus_data[0];
q1 = kf->xhatminus_data[1];
q2 = kf->xhatminus_data[2];
q3 = kf->xhatminus_data[3];
kf->temp_vector.numRows = kf->H.numRows;
kf->temp_vector.numCols = 1;
/* 计算预测得到的重力加速度方向（通过姿态获取） */
kf->temp_vector_data[0] = 2 * (q1 * q3 - q0 * q2);
kf->temp_vector_data[1] = 2 * (q0 * q1 + q2 * q3);
/* temp_vector = h(xhat'(k)) */
kf->temp_vector_data[2] = q0 * q0 - q1 * q1 - q2 * q2 + q3 * q3;
/* 计算预测值和各个轴的方向余弦 */
for (uint8_t i = 0; i < 3; i++) {
QEKF_INS.orientation_cosine[i] = acosf(fabsf(kf->temp_vector_data[i]));
}
/* 利用加速度计数据修正 */
kf->temp_vector1.numRows = kf->z.numRows;
kf->temp_vector1.numCols = 1;
/* temp_vector1 = z(k) - h(xhat'(k)) */
kf->mat_status = KF_MatSub(&kf->z, &kf->temp_vector, &kf->temp_vector1);
/* 卡方检验 */
kf->temp_matrix.numRows = kf->temp_vector1.numRows;
kf->temp_matrix.numCols = 1;
/* temp_matrix = inv(H·P'(k)·HT + R)·(z(k) - h(xhat'(k))) */
kf->mat_status =
KF_MatMult(&kf->temp_matrix1, &kf->temp_vector1, &kf->temp_matrix);
kf->temp_vector.numRows = 1;
kf->temp_vector.numCols = kf->temp_vector1.numRows;
/* temp_vector = (z(k) - h(xhat'(k)))' */
kf->mat_status = KF_MatTrans(&kf->temp_vector1, &kf->temp_vector);
kf->mat_status =
KF_MatMult(&kf->temp_vector, &kf->temp_matrix, &QEKF_INS.chi_square);
/* rk较小，滤波器已收敛/正在收敛 */
if (QEKF_INS.chi_square_data[0] < 0.5f * QEKF_INS.chi_square_threshold) {
QEKF_INS.converge_flag = 1;
}
/* rk大于阈值但曾经收敛过 */
if (QEKF_INS.chi_square_data[0] > QEKF_INS.chi_square_threshold &&
QEKF_INS.converge_flag) {
if (QEKF_INS.stable_flag) {
QEKF_INS.error_count++; /* 载体静止时仍无法通过卡方检验 */
} else {
QEKF_INS.error_count = 0;
}
if (QEKF_INS.error_count > 50) {
/* 滤波器发散 */
QEKF_INS.converge_flag = 0;
kf->skip_eq5 = FALSE; /* 步骤5是协方差矩阵P更新 */
} else {
/* 残差未通过卡方检验，仅预测 */
/* xhat(k) = xhat'(k) */
/* P(k) = P'(k) */
memcpy(kf->xhat_data, kf->xhatminus_data,
sizeof(float) * kf->xhat_size);
memcpy(kf->P_data, kf->Pminus_data,
sizeof(float) * kf->xhat_size * kf->xhat_size);
kf->skip_eq5 = TRUE; /* 跳过P更新 */
return;
}
} else {
/* 发散或rk不大/可接受，使用自适应增益 */
/* 自适应缩放，rk越小则增益越大，否则更相信预测值 */
if (QEKF_INS.chi_square_data[0] > 0.1f * QEKF_INS.chi_square_threshold &&
QEKF_INS.converge_flag) {
QEKF_INS.adaptive_gain_scale =
(QEKF_INS.chi_square_threshold - QEKF_INS.chi_square_data[0]) /
(0.9f * QEKF_INS.chi_square_threshold);
} else {
QEKF_INS.adaptive_gain_scale = 1;
}
QEKF_INS.error_count = 0;
kf->skip_eq5 = FALSE;
}
/* 计算卡尔曼增益K */
kf->temp_matrix.numRows = kf->Pminus.numRows;
kf->temp_matrix.numCols = kf->HT.numCols;
/* temp_matrix = P'(k)·HT */
kf->mat_status = KF_MatMult(&kf->Pminus, &kf->HT, &kf->temp_matrix);
kf->mat_status = KF_MatMult(&kf->temp_matrix, &kf->temp_matrix1, &kf->K);
/* 应用自适应增益 */
for (uint8_t i = 0; i < kf->K.numRows * kf->K.numCols; i++) {
kf->K_data[i] *= QEKF_INS.adaptive_gain_scale;
}
for (uint8_t i = 4; i < 6; i++) {
for (uint8_t j = 0; j < 3; j++) {
kf->K_data[i * 3 + j] *=
QEKF_INS.orientation_cosine[i - 4] / 1.5707963f; /* 1 rad */
}
}
kf->temp_vector.numRows = kf->K.numRows;
kf->temp_vector.numCols = 1;
/* temp_vector = K(k)·(z(k) - H·xhat'(k)) */
kf->mat_status = KF_MatMult(&kf->K, &kf->temp_vector1, &kf->temp_vector);
/* 零偏修正限幅，一般不会有过大的漂移 */
if (QEKF_INS.converge_flag) {
for (uint8_t i = 4; i < 6; i++) {
if (kf->temp_vector.pData[i] > 1e-2f * QEKF_INS.dt) {
kf->temp_vector.pData[i] = 1e-2f * QEKF_INS.dt;
}
if (kf->temp_vector.pData[i] < -1e-2f * QEKF_INS.dt) {
kf->temp_vector.pData[i] = -1e-2f * QEKF_INS.dt;
}
}
}
/* 不修正yaw轴数据 */
kf->temp_vector.pData[3] = 0;
kf->mat_status = KF_MatAdd(&kf->xhatminus, &kf->temp_vector, &kf->xhat);
}
/**
* @brief EKF观测环节，复制数据用于调试
*
* @param kf 卡尔曼滤波器结构体指针
*/
static void QEKF_Observe(KF_t *kf) {
memcpy(QEKF_P_Init, kf->P_data, sizeof(QEKF_P_Init));
memcpy(QEKF_K, kf->K_data, sizeof(QEKF_K));
memcpy(QEKF_H, kf->H_data, sizeof(QEKF_H));
}
/**
* @brief 快速计算1/sqrt(x)
*
* @param x 输入值
* @return float 1/sqrt(x)的近似值
*/
static float QEKF_InvSqrt(float x) {
float halfx = 0.5f * x;
float y = x;
long i = *(long *)&y;
i = 0x5f375a86 - (i >> 1);
y = *(float *)&i;
y = y * (1.5f - (halfx * y * y));
return y;
}
/* USER FUNCTION BEGIN */
/* USER FUNCTION END */

111
User/component/QuaternionEKF.h Normal file
View File

@ -0,0 +1,111 @@
/*
四元数扩展卡尔曼滤波器（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

274
User/component/kalman_filter.c
View File

@ -1,46 +1,46 @@
/* /*
卡尔曼滤波器 modified from wang hongxi 卡尔曼滤波器 modified from wang hongxi
支持动态量测调整，使用ARM CMSIS DSP优化矩阵运算 支持动态量测调整，使用ARM CMSIS DSP优化矩阵运算
主要特性： 主要特性：
- 基于量测有效性的 H、R、K 矩阵动态调整 - 基于量测有效性的 H、R、K 矩阵动态调整
- 支持不同传感器采样频率 - 支持不同传感器采样频率
- 矩阵 P 防过度收敛机制 - 矩阵 P 防过度收敛机制
- ARM CMSIS DSP 优化的矩阵运算 - ARM CMSIS DSP 优化的矩阵运算
- 可扩展架构，支持用户自定义函数（EKF/UKF/ESKF） - 可扩展架构，支持用户自定义函数（EKF/UKF/ESKF）
使用说明： 使用说明：
1. 矩阵初始化：P、F、Q 使用标准初始化方式 1. 矩阵初始化：P、F、Q 使用标准初始化方式
H、R 在使用自动调整时需要配置量测映射 H、R 在使用自动调整时需要配置量测映射
2. 自动调整模式 (use_auto_adjustment = 1)： 2. 自动调整模式 (use_auto_adjustment = 1)：
- 提供 measurement_map：每个量测对应的状态索引 - 提供 measurement_map：每个量测对应的状态索引
- 提供 measurement_degree：H 矩阵元素值 - 提供 measurement_degree：H 矩阵元素值
- 提供 mat_r_diagonal_elements：量测噪声方差 - 提供 mat_r_diagonal_elements：量测噪声方差
3. 固定模式 (use_auto_adjustment = 0)： 3. 固定模式 (use_auto_adjustment = 0)：
- 像初始化 P 矩阵那样正常初始化 z、H、R - 像初始化 P 矩阵那样正常初始化 z、H、R
4. 量测更新： 4. 量测更新：
- 在传感器回调函数中更新 measured_vector - 在传感器回调函数中更新 measured_vector
- 值为 0 表示量测无效 - 值为 0 表示量测无效
- 向量在每次 KF 更新后会被重置为 0 - 向量在每次 KF 更新后会被重置为 0
5. 防过度收敛： 5. 防过度收敛：
- 设置 state_min_variance 防止 P 矩阵过度收敛 - 设置 state_min_variance 防止 P 矩阵过度收敛
- 帮助滤波器适应缓慢变化的状态 - 帮助滤波器适应缓慢变化的状态
使用示例：高度估计 使用示例：高度估计
状态向量 x = 状态向量 x =
| 高度 | | 高度 |
| 速度 | | 速度 |
| 加速度 | | 加速度 |
KF_t Height_KF; KF_t Height_KF;
void INS_Task_Init(void) void INS_Task_Init(void)
{ {
// 初始协方差矩阵 P // 初始协方差矩阵 P
static float P_Init[9] = static float P_Init[9] =
{ {
10, 0, 0, 10, 0, 0,
@ -48,7 +48,7 @@
0, 0, 10, 0, 0, 10,
}; };
// 状态转移矩阵 F（根据运动学模型） // 状态转移矩阵 F（根据运动学模型）
static float F_Init[9] = static float F_Init[9] =
{ {
1, dt, 0.5*dt*dt, 1, dt, 0.5*dt*dt,
@ -56,7 +56,7 @@
0, 0, 1, 0, 0, 1,
}; };
// 过程噪声协方差矩阵 Q // 过程噪声协方差矩阵 Q
static float Q_Init[9] = static float Q_Init[9] =
{ {
0.25*dt*dt*dt*dt, 0.5*dt*dt*dt, 0.5*dt*dt, 0.25*dt*dt*dt*dt, 0.5*dt*dt*dt, 0.5*dt*dt,
@ -64,35 +64,35 @@
0.5*dt*dt, dt, 1, 0.5*dt*dt, dt, 1,
}; };
// 设置状态最小方差（防止过度收敛） // 设置状态最小方差（防止过度收敛）
static float state_min_variance[3] = {0.03, 0.005, 0.1}; static float state_min_variance[3] = {0.03, 0.005, 0.1};
// 开启自动调整 // 开启自动调整
Height_KF.use_auto_adjustment = 1; Height_KF.use_auto_adjustment = 1;
// 量测映射：[气压高度对应状态1, GPS高度对应状态1, 加速度计对应状态3] // 量测映射：[气压高度对应状态1, GPS高度对应状态1, 加速度计对应状态3]
static uint8_t measurement_reference[3] = {1, 1, 3}; static uint8_t measurement_reference[3] = {1, 1, 3};
// 量测系数（H矩阵元素值） // 量测系数（H矩阵元素值）
static float measurement_degree[3] = {1, 1, 1}; static float measurement_degree[3] = {1, 1, 1};
// 根据 measurement_reference 与 measurement_degree 生成 H 矩阵如下 // 根据 measurement_reference 与 measurement_degree 生成 H 矩阵如下
// （在当前周期全部量测数据有效的情况下） // （在当前周期全部量测数据有效的情况下）
// |1 0 0| // |1 0 0|
// |1 0 0| // |1 0 0|
// |0 0 1| // |0 0 1|
// 量测噪声方差（R矩阵对角元素） // 量测噪声方差（R矩阵对角元素）
static float mat_r_diagonal_elements[3] = {30, 25, 35}; static float mat_r_diagonal_elements[3] = {30, 25, 35};
// 根据 mat_r_diagonal_elements 生成 R 矩阵如下 // 根据 mat_r_diagonal_elements 生成 R 矩阵如下
// （在当前周期全部量测数据有效的情况下） // （在当前周期全部量测数据有效的情况下）
// |30 0 0| // |30 0 0|
// | 0 25 0| // | 0 25 0|
// | 0 0 35| // | 0 0 35|
// 初始化卡尔曼滤波器（状态维数3，控制维数0，量测维数3） // 初始化卡尔曼滤波器（状态维数3，控制维数0，量测维数3）
KF_Init(&Height_KF, 3, 0, 3); KF_Init(&Height_KF, 3, 0, 3);
// 设置矩阵初值 // 设置矩阵初值
memcpy(Height_KF.P_data, P_Init, sizeof(P_Init)); memcpy(Height_KF.P_data, P_Init, sizeof(P_Init));
memcpy(Height_KF.F_data, F_Init, sizeof(F_Init)); memcpy(Height_KF.F_data, F_Init, sizeof(F_Init));
memcpy(Height_KF.Q_data, Q_Init, sizeof(Q_Init)); memcpy(Height_KF.Q_data, Q_Init, sizeof(Q_Init));
@ -108,28 +108,28 @@
void INS_Task(void const *pvParameters) void INS_Task(void const *pvParameters)
{ {
// 循环更新卡尔曼滤波器 // 循环更新卡尔曼滤波器
KF_Update(&Height_KF); KF_Update(&Height_KF);
vTaskDelay(ts); vTaskDelay(ts);
} }
// 传感器回调函数示例：在数据就绪时更新 measured_vector // 传感器回调函数示例：在数据就绪时更新 measured_vector
void Barometer_Read_Over(void) void Barometer_Read_Over(void)
{ {
...... ......
INS_KF.measured_vector[0] = baro_height; // 气压计高度 INS_KF.measured_vector[0] = baro_height; // 气压计高度
} }
void GPS_Read_Over(void) void GPS_Read_Over(void)
{ {
...... ......
INS_KF.measured_vector[1] = GPS_height; // GPS高度 INS_KF.measured_vector[1] = GPS_height; // GPS高度
} }
void Acc_Data_Process(void) void Acc_Data_Process(void)
{ {
...... ......
INS_KF.measured_vector[2] = acc.z; // Z轴加速度 INS_KF.measured_vector[2] = acc.z; // Z轴加速度
} }
*/ */
@ -143,17 +143,17 @@
/* USER DEFINE END */ /* USER DEFINE END */
/* 私有函数声明 */ /* 私有函数声明 */
static void KF_AdjustHKR(KF_t *kf); static void KF_AdjustHKR(KF_t *kf);
/** /**
* @brief 初始化卡尔曼滤波器并分配矩阵内存 * @brief 初始化卡尔曼滤波器并分配矩阵内存
* *
* @param kf 卡尔曼滤波器结构体指针 * @param kf 卡尔曼滤波器结构体指针
* @param xhat_size 状态向量维度 * @param xhat_size 状态向量维度
* @param u_size 控制向量维度（无控制输入时设为0） * @param u_size 控制向量维度（无控制输入时设为0）
* @param z_size 量测向量维度 * @param z_size 量测向量维度
* @return int8_t 0对应没有错误 * @return int8_t 0对应没有错误
*/ */
int8_t KF_Init(KF_t *kf, uint8_t xhat_size, uint8_t u_size, uint8_t z_size) { int8_t KF_Init(KF_t *kf, uint8_t xhat_size, uint8_t u_size, uint8_t z_size) {
if (kf == NULL) return -1; if (kf == NULL) return -1;
@ -164,7 +164,7 @@ int8_t KF_Init(KF_t *kf, uint8_t xhat_size, uint8_t u_size, uint8_t z_size) {
kf->measurement_valid_num = 0; kf->measurement_valid_num = 0;
/* 量测标志分配 */ /* 量测标志分配 */
kf->measurement_map = (uint8_t *)user_malloc(sizeof(uint8_t) * z_size); kf->measurement_map = (uint8_t *)user_malloc(sizeof(uint8_t) * z_size);
memset(kf->measurement_map, 0, sizeof(uint8_t) * z_size); memset(kf->measurement_map, 0, sizeof(uint8_t) * z_size);
@ -180,7 +180,7 @@ int8_t KF_Init(KF_t *kf, uint8_t xhat_size, uint8_t u_size, uint8_t z_size) {
kf->temp = (uint8_t *)user_malloc(sizeof(uint8_t) * z_size); kf->temp = (uint8_t *)user_malloc(sizeof(uint8_t) * z_size);
memset(kf->temp, 0, sizeof(uint8_t) * z_size); memset(kf->temp, 0, sizeof(uint8_t) * z_size);
/* 滤波数据分配 */ /* 滤波数据分配 */
kf->filtered_value = (float *)user_malloc(sizeof(float) * xhat_size); kf->filtered_value = (float *)user_malloc(sizeof(float) * xhat_size);
memset(kf->filtered_value, 0, sizeof(float) * xhat_size); memset(kf->filtered_value, 0, sizeof(float) * xhat_size);
@ -190,39 +190,39 @@ int8_t KF_Init(KF_t *kf, uint8_t xhat_size, uint8_t u_size, uint8_t z_size) {
kf->control_vector = (float *)user_malloc(sizeof(float) * u_size); kf->control_vector = (float *)user_malloc(sizeof(float) * u_size);
memset(kf->control_vector, 0, sizeof(float) * u_size); memset(kf->control_vector, 0, sizeof(float) * u_size);
/* 状态估计 xhat x(k|k) */ /* 状态估计 xhat x(k|k) */
kf->xhat_data = (float *)user_malloc(sizeof(float) * xhat_size); kf->xhat_data = (float *)user_malloc(sizeof(float) * xhat_size);
memset(kf->xhat_data, 0, sizeof(float) * xhat_size); memset(kf->xhat_data, 0, sizeof(float) * xhat_size);
KF_MatInit(&kf->xhat, kf->xhat_size, 1, kf->xhat_data); KF_MatInit(&kf->xhat, kf->xhat_size, 1, kf->xhat_data);
/* 先验状态估计 xhatminus x(k|k-1) */ /* 先验状态估计 xhatminus x(k|k-1) */
kf->xhatminus_data = (float *)user_malloc(sizeof(float) * xhat_size); kf->xhatminus_data = (float *)user_malloc(sizeof(float) * xhat_size);
memset(kf->xhatminus_data, 0, sizeof(float) * xhat_size); memset(kf->xhatminus_data, 0, sizeof(float) * xhat_size);
KF_MatInit(&kf->xhatminus, kf->xhat_size, 1, kf->xhatminus_data); KF_MatInit(&kf->xhatminus, kf->xhat_size, 1, kf->xhatminus_data);
/* 控制向量 u */ /* 控制向量 u */
if (u_size != 0) { if (u_size != 0) {
kf->u_data = (float *)user_malloc(sizeof(float) * u_size); kf->u_data = (float *)user_malloc(sizeof(float) * u_size);
memset(kf->u_data, 0, sizeof(float) * u_size); memset(kf->u_data, 0, sizeof(float) * u_size);
KF_MatInit(&kf->u, kf->u_size, 1, kf->u_data); KF_MatInit(&kf->u, kf->u_size, 1, kf->u_data);
} }
/* 量测向量 z */ /* 量测向量 z */
kf->z_data = (float *)user_malloc(sizeof(float) * z_size); kf->z_data = (float *)user_malloc(sizeof(float) * z_size);
memset(kf->z_data, 0, sizeof(float) * z_size); memset(kf->z_data, 0, sizeof(float) * z_size);
KF_MatInit(&kf->z, kf->z_size, 1, kf->z_data); KF_MatInit(&kf->z, kf->z_size, 1, kf->z_data);
/* 协方差矩阵 P(k|k) */ /* 协方差矩阵 P(k|k) */
kf->P_data = (float *)user_malloc(sizeof(float) * xhat_size * xhat_size); kf->P_data = (float *)user_malloc(sizeof(float) * xhat_size * xhat_size);
memset(kf->P_data, 0, sizeof(float) * xhat_size * xhat_size); memset(kf->P_data, 0, sizeof(float) * xhat_size * xhat_size);
KF_MatInit(&kf->P, kf->xhat_size, kf->xhat_size, kf->P_data); KF_MatInit(&kf->P, kf->xhat_size, kf->xhat_size, kf->P_data);
/* 先验协方差矩阵 P(k|k-1) */ /* 先验协方差矩阵 P(k|k-1) */
kf->Pminus_data = (float *)user_malloc(sizeof(float) * xhat_size * xhat_size); kf->Pminus_data = (float *)user_malloc(sizeof(float) * xhat_size * xhat_size);
memset(kf->Pminus_data, 0, sizeof(float) * xhat_size * xhat_size); memset(kf->Pminus_data, 0, sizeof(float) * xhat_size * xhat_size);
KF_MatInit(&kf->Pminus, kf->xhat_size, kf->xhat_size, kf->Pminus_data); KF_MatInit(&kf->Pminus, kf->xhat_size, kf->xhat_size, kf->Pminus_data);
/* 状态转移矩阵 F 及其转置 FT */ /* 状态转移矩阵 F 及其转置 FT */
kf->F_data = (float *)user_malloc(sizeof(float) * xhat_size * xhat_size); kf->F_data = (float *)user_malloc(sizeof(float) * xhat_size * xhat_size);
kf->FT_data = (float *)user_malloc(sizeof(float) * xhat_size * xhat_size); kf->FT_data = (float *)user_malloc(sizeof(float) * xhat_size * xhat_size);
memset(kf->F_data, 0, sizeof(float) * xhat_size * xhat_size); memset(kf->F_data, 0, sizeof(float) * xhat_size * xhat_size);
@ -230,14 +230,14 @@ int8_t KF_Init(KF_t *kf, uint8_t xhat_size, uint8_t u_size, uint8_t z_size) {
KF_MatInit(&kf->F, kf->xhat_size, kf->xhat_size, kf->F_data); KF_MatInit(&kf->F, kf->xhat_size, kf->xhat_size, kf->F_data);
KF_MatInit(&kf->FT, kf->xhat_size, kf->xhat_size, kf->FT_data); KF_MatInit(&kf->FT, kf->xhat_size, kf->xhat_size, kf->FT_data);
/* 控制矩阵 B */ /* 控制矩阵 B */
if (u_size != 0) { if (u_size != 0) {
kf->B_data = (float *)user_malloc(sizeof(float) * xhat_size * u_size); kf->B_data = (float *)user_malloc(sizeof(float) * xhat_size * u_size);
memset(kf->B_data, 0, sizeof(float) * xhat_size * u_size); memset(kf->B_data, 0, sizeof(float) * xhat_size * u_size);
KF_MatInit(&kf->B, kf->xhat_size, kf->u_size, kf->B_data); KF_MatInit(&kf->B, kf->xhat_size, kf->u_size, kf->B_data);
} }
/* 量测矩阵 H 及其转置 HT */ /* 量测矩阵 H 及其转置 HT */
kf->H_data = (float *)user_malloc(sizeof(float) * z_size * xhat_size); kf->H_data = (float *)user_malloc(sizeof(float) * z_size * xhat_size);
kf->HT_data = (float *)user_malloc(sizeof(float) * xhat_size * z_size); kf->HT_data = (float *)user_malloc(sizeof(float) * xhat_size * z_size);
memset(kf->H_data, 0, sizeof(float) * z_size * xhat_size); memset(kf->H_data, 0, sizeof(float) * z_size * xhat_size);
@ -245,22 +245,22 @@ int8_t KF_Init(KF_t *kf, uint8_t xhat_size, uint8_t u_size, uint8_t z_size) {
KF_MatInit(&kf->H, kf->z_size, kf->xhat_size, kf->H_data); KF_MatInit(&kf->H, kf->z_size, kf->xhat_size, kf->H_data);
KF_MatInit(&kf->HT, kf->xhat_size, kf->z_size, kf->HT_data); KF_MatInit(&kf->HT, kf->xhat_size, kf->z_size, kf->HT_data);
/* 过程噪声协方差矩阵 Q */ /* 过程噪声协方差矩阵 Q */
kf->Q_data = (float *)user_malloc(sizeof(float) * xhat_size * xhat_size); kf->Q_data = (float *)user_malloc(sizeof(float) * xhat_size * xhat_size);
memset(kf->Q_data, 0, sizeof(float) * xhat_size * xhat_size); memset(kf->Q_data, 0, sizeof(float) * xhat_size * xhat_size);
KF_MatInit(&kf->Q, kf->xhat_size, kf->xhat_size, kf->Q_data); KF_MatInit(&kf->Q, kf->xhat_size, kf->xhat_size, kf->Q_data);
/* 量测噪声协方差矩阵 R */ /* 量测噪声协方差矩阵 R */
kf->R_data = (float *)user_malloc(sizeof(float) * z_size * z_size); kf->R_data = (float *)user_malloc(sizeof(float) * z_size * z_size);
memset(kf->R_data, 0, sizeof(float) * z_size * z_size); memset(kf->R_data, 0, sizeof(float) * z_size * z_size);
KF_MatInit(&kf->R, kf->z_size, kf->z_size, kf->R_data); KF_MatInit(&kf->R, kf->z_size, kf->z_size, kf->R_data);
/* 卡尔曼增益 K */ /* 卡尔曼增益 K */
kf->K_data = (float *)user_malloc(sizeof(float) * xhat_size * z_size); kf->K_data = (float *)user_malloc(sizeof(float) * xhat_size * z_size);
memset(kf->K_data, 0, sizeof(float) * xhat_size * z_size); memset(kf->K_data, 0, sizeof(float) * xhat_size * z_size);
KF_MatInit(&kf->K, kf->xhat_size, kf->z_size, kf->K_data); KF_MatInit(&kf->K, kf->xhat_size, kf->z_size, kf->K_data);
/* 临时矩阵分配 */ /* 临时矩阵分配 */
kf->S_data = (float *)user_malloc(sizeof(float) * xhat_size * xhat_size); kf->S_data = (float *)user_malloc(sizeof(float) * xhat_size * xhat_size);
kf->temp_matrix_data = kf->temp_matrix_data =
(float *)user_malloc(sizeof(float) * xhat_size * xhat_size); (float *)user_malloc(sizeof(float) * xhat_size * xhat_size);
@ -277,14 +277,14 @@ int8_t KF_Init(KF_t *kf, uint8_t xhat_size, uint8_t u_size, uint8_t z_size) {
KF_MatInit(&kf->temp_vector, kf->xhat_size, 1, kf->temp_vector_data); KF_MatInit(&kf->temp_vector, kf->xhat_size, 1, kf->temp_vector_data);
KF_MatInit(&kf->temp_vector1, kf->xhat_size, 1, kf->temp_vector_data1); KF_MatInit(&kf->temp_vector1, kf->xhat_size, 1, kf->temp_vector_data1);
/* 初始化跳过标志 */ /* 初始化跳过标志 */
kf->skip_eq1 = 0; kf->skip_eq1 = 0;
kf->skip_eq2 = 0; kf->skip_eq2 = 0;
kf->skip_eq3 = 0; kf->skip_eq3 = 0;
kf->skip_eq4 = 0; kf->skip_eq4 = 0;
kf->skip_eq5 = 0; kf->skip_eq5 = 0;
/* 初始化用户函数指针 */ /* 初始化用户函数指针 */
kf->User_Func0_f = NULL; kf->User_Func0_f = NULL;
kf->User_Func1_f = NULL; kf->User_Func1_f = NULL;
kf->User_Func2_f = NULL; kf->User_Func2_f = NULL;
@ -297,15 +297,15 @@ int8_t KF_Init(KF_t *kf, uint8_t xhat_size, uint8_t u_size, uint8_t z_size) {
} }
/** /**
* @brief 获取量测并在启用自动调整时调整矩阵 * @brief 获取量测并在启用自动调整时调整矩阵
* *
* @param kf 卡尔曼滤波器结构体指针 * @param kf 卡尔曼滤波器结构体指针
* @return int8_t 0对应没有错误 * @return int8_t 0对应没有错误
*/ */
int8_t KF_Measure(KF_t *kf) { int8_t KF_Measure(KF_t *kf) {
if (kf == NULL) return -1; if (kf == NULL) return -1;
/* 根据量测有效性自动调整 H, K, R 矩阵 */ /* 根据量测有效性自动调整 H, K, R 矩阵 */
if (kf->use_auto_adjustment != 0) { if (kf->use_auto_adjustment != 0) {
KF_AdjustHKR(kf); KF_AdjustHKR(kf);
} else { } else {
@ -319,17 +319,17 @@ int8_t KF_Measure(KF_t *kf) {
} }
/** /**
* @brief 步骤1：先验状态估计 - xhat'(k) = F·xhat(k-1) + B·u * @brief 步骤1：先验状态估计 - xhat'(k) = F·xhat(k-1) + B·u
* *
* @param kf 卡尔曼滤波器结构体指针 * @param kf 卡尔曼滤波器结构体指针
* @return int8_t 0对应没有错误 * @return int8_t 0对应没有错误
*/ */
int8_t KF_PredictState(KF_t *kf) { int8_t KF_PredictState(KF_t *kf) {
if (kf == NULL) return -1; if (kf == NULL) return -1;
if (!kf->skip_eq1) { if (!kf->skip_eq1) {
if (kf->u_size > 0) { if (kf->u_size > 0) {
/* 有控制输入: xhat'(k) = F·xhat(k-1) + B·u */ /* 有控制输入: xhat'(k) = F·xhat(k-1) + B·u */
kf->temp_vector.numRows = kf->xhat_size; kf->temp_vector.numRows = kf->xhat_size;
kf->temp_vector.numCols = 1; kf->temp_vector.numCols = 1;
kf->mat_status = KF_MatMult(&kf->F, &kf->xhat, &kf->temp_vector); kf->mat_status = KF_MatMult(&kf->F, &kf->xhat, &kf->temp_vector);
@ -340,7 +340,7 @@ int8_t KF_PredictState(KF_t *kf) {
kf->mat_status = kf->mat_status =
KF_MatAdd(&kf->temp_vector, &kf->temp_vector1, &kf->xhatminus); KF_MatAdd(&kf->temp_vector, &kf->temp_vector1, &kf->xhatminus);
} else { } else {
/* 无控制输入: xhat'(k) = F·xhat(k-1) */ /* 无控制输入: xhat'(k) = F·xhat(k-1) */
kf->mat_status = KF_MatMult(&kf->F, &kf->xhat, &kf->xhatminus); kf->mat_status = KF_MatMult(&kf->F, &kf->xhat, &kf->xhatminus);
} }
} }
@ -349,10 +349,10 @@ int8_t KF_PredictState(KF_t *kf) {
} }
/** /**
* @brief 步骤2：先验协方差 - P'(k) = F·P(k-1)·F^T + Q * @brief 步骤2：先验协方差 - P'(k) = F·P(k-1)·F^T + Q
* *
* @param kf 卡尔曼滤波器结构体指针 * @param kf 卡尔曼滤波器结构体指针
* @return int8_t 0对应没有错误 * @return int8_t 0对应没有错误
*/ */
int8_t KF_PredictCovariance(KF_t *kf) { int8_t KF_PredictCovariance(KF_t *kf) {
if (kf == NULL) return -1; if (kf == NULL) return -1;
@ -362,7 +362,7 @@ int8_t KF_PredictCovariance(KF_t *kf) {
kf->mat_status = KF_MatMult(&kf->F, &kf->P, &kf->Pminus); kf->mat_status = KF_MatMult(&kf->F, &kf->P, &kf->Pminus);
kf->temp_matrix.numRows = kf->Pminus.numRows; kf->temp_matrix.numRows = kf->Pminus.numRows;
kf->temp_matrix.numCols = kf->FT.numCols; kf->temp_matrix.numCols = kf->FT.numCols;
/* F·P(k-1)·F^T */ /* F·P(k-1)·F^T */
kf->mat_status = KF_MatMult(&kf->Pminus, &kf->FT, &kf->temp_matrix); kf->mat_status = KF_MatMult(&kf->Pminus, &kf->FT, &kf->temp_matrix);
kf->mat_status = KF_MatAdd(&kf->temp_matrix, &kf->Q, &kf->Pminus); kf->mat_status = KF_MatAdd(&kf->temp_matrix, &kf->Q, &kf->Pminus);
} }
@ -371,10 +371,10 @@ int8_t KF_PredictCovariance(KF_t *kf) {
} }
/** /**
* @brief 步骤3：卡尔曼增益 - K = P'(k)·H^T / (H·P'(k)·H^T + R) * @brief 步骤3：卡尔曼增益 - K = P'(k)·H^T / (H·P'(k)·H^T + R)
* *
* @param kf 卡尔曼滤波器结构体指针 * @param kf 卡尔曼滤波器结构体指针
* @return int8_t 0对应没有错误 * @return int8_t 0对应没有错误
*/ */
int8_t KF_CalcGain(KF_t *kf) { int8_t KF_CalcGain(KF_t *kf) {
if (kf == NULL) return -1; if (kf == NULL) return -1;
@ -383,23 +383,23 @@ int8_t KF_CalcGain(KF_t *kf) {
kf->mat_status = KF_MatTrans(&kf->H, &kf->HT); kf->mat_status = KF_MatTrans(&kf->H, &kf->HT);
kf->temp_matrix.numRows = kf->H.numRows; kf->temp_matrix.numRows = kf->H.numRows;
kf->temp_matrix.numCols = kf->Pminus.numCols; kf->temp_matrix.numCols = kf->Pminus.numCols;
/* H·P'(k) */ /* H·P'(k) */
kf->mat_status = KF_MatMult(&kf->H, &kf->Pminus, &kf->temp_matrix); kf->mat_status = KF_MatMult(&kf->H, &kf->Pminus, &kf->temp_matrix);
kf->temp_matrix1.numRows = kf->temp_matrix.numRows; kf->temp_matrix1.numRows = kf->temp_matrix.numRows;
kf->temp_matrix1.numCols = kf->HT.numCols; kf->temp_matrix1.numCols = kf->HT.numCols;
/* H·P'(k)·H^T */ /* H·P'(k)·H^T */
kf->mat_status = KF_MatMult(&kf->temp_matrix, &kf->HT, &kf->temp_matrix1); kf->mat_status = KF_MatMult(&kf->temp_matrix, &kf->HT, &kf->temp_matrix1);
kf->S.numRows = kf->R.numRows; kf->S.numRows = kf->R.numRows;
kf->S.numCols = kf->R.numCols; kf->S.numCols = kf->R.numCols;
/* S = H·P'(k)·H^T + R */ /* S = H·P'(k)·H^T + R */
kf->mat_status = KF_MatAdd(&kf->temp_matrix1, &kf->R, &kf->S); kf->mat_status = KF_MatAdd(&kf->temp_matrix1, &kf->R, &kf->S);
/* S^-1 */ /* S^-1 */
kf->mat_status = KF_MatInv(&kf->S, &kf->temp_matrix1); kf->mat_status = KF_MatInv(&kf->S, &kf->temp_matrix1);
kf->temp_matrix.numRows = kf->Pminus.numRows; kf->temp_matrix.numRows = kf->Pminus.numRows;
kf->temp_matrix.numCols = kf->HT.numCols; kf->temp_matrix.numCols = kf->HT.numCols;
/* P'(k)·H^T */ /* P'(k)·H^T */
kf->mat_status = KF_MatMult(&kf->Pminus, &kf->HT, &kf->temp_matrix); kf->mat_status = KF_MatMult(&kf->Pminus, &kf->HT, &kf->temp_matrix);
/* K = P'(k)·H^T·S^-1 */ /* K = P'(k)·H^T·S^-1 */
kf->mat_status = KF_MatMult(&kf->temp_matrix, &kf->temp_matrix1, &kf->K); kf->mat_status = KF_MatMult(&kf->temp_matrix, &kf->temp_matrix1, &kf->K);
} }
@ -407,10 +407,10 @@ int8_t KF_CalcGain(KF_t *kf) {
} }
/** /**
* @brief 步骤4：状态更新 - xhat(k) = xhat'(k) + K·(z - H·xhat'(k)) * @brief 步骤4：状态更新 - xhat(k) = xhat'(k) + K·(z - H·xhat'(k))
* *
* @param kf 卡尔曼滤波器结构体指针 * @param kf 卡尔曼滤波器结构体指针
* @return int8_t 0对应没有错误 * @return int8_t 0对应没有错误
*/ */
int8_t KF_UpdateState(KF_t *kf) { int8_t KF_UpdateState(KF_t *kf) {
if (kf == NULL) return -1; if (kf == NULL) return -1;
@ -418,17 +418,17 @@ int8_t KF_UpdateState(KF_t *kf) {
if (!kf->skip_eq4) { if (!kf->skip_eq4) {
kf->temp_vector.numRows = kf->H.numRows; kf->temp_vector.numRows = kf->H.numRows;
kf->temp_vector.numCols = 1; kf->temp_vector.numCols = 1;
/* H·xhat'(k) */ /* H·xhat'(k) */
kf->mat_status = KF_MatMult(&kf->H, &kf->xhatminus, &kf->temp_vector); kf->mat_status = KF_MatMult(&kf->H, &kf->xhatminus, &kf->temp_vector);
kf->temp_vector1.numRows = kf->z.numRows; kf->temp_vector1.numRows = kf->z.numRows;
kf->temp_vector1.numCols = 1; kf->temp_vector1.numCols = 1;
/* 新息: z - H·xhat'(k) */ /* 新息: z - H·xhat'(k) */
kf->mat_status = KF_MatSub(&kf->z, &kf->temp_vector, &kf->temp_vector1); kf->mat_status = KF_MatSub(&kf->z, &kf->temp_vector, &kf->temp_vector1);
kf->temp_vector.numRows = kf->K.numRows; kf->temp_vector.numRows = kf->K.numRows;
kf->temp_vector.numCols = 1; kf->temp_vector.numCols = 1;
/* K·新息 */ /* K·新息 */
kf->mat_status = KF_MatMult(&kf->K, &kf->temp_vector1, &kf->temp_vector); kf->mat_status = KF_MatMult(&kf->K, &kf->temp_vector1, &kf->temp_vector);
/* xhat = xhat' + K·新息 */ /* xhat = xhat' + K·新息 */
kf->mat_status = KF_MatAdd(&kf->xhatminus, &kf->temp_vector, &kf->xhat); kf->mat_status = KF_MatAdd(&kf->xhatminus, &kf->temp_vector, &kf->xhat);
} }
@ -436,10 +436,10 @@ int8_t KF_UpdateState(KF_t *kf) {
} }
/** /**
* @brief 步骤5：协方差更新 - P(k) = P'(k) - K·H·P'(k) * @brief 步骤5：协方差更新 - P(k) = P'(k) - K·H·P'(k)
* *
* @param kf 卡尔曼滤波器结构体指针 * @param kf 卡尔曼滤波器结构体指针
* @return int8_t 0对应没有错误 * @return int8_t 0对应没有错误
*/ */
int8_t KF_UpdateCovariance(KF_t *kf) { int8_t KF_UpdateCovariance(KF_t *kf) {
if (kf == NULL) return -1; if (kf == NULL) return -1;
@ -449,11 +449,11 @@ int8_t KF_UpdateCovariance(KF_t *kf) {
kf->temp_matrix.numCols = kf->H.numCols; kf->temp_matrix.numCols = kf->H.numCols;
kf->temp_matrix1.numRows = kf->temp_matrix.numRows; kf->temp_matrix1.numRows = kf->temp_matrix.numRows;
kf->temp_matrix1.numCols = kf->Pminus.numCols; kf->temp_matrix1.numCols = kf->Pminus.numCols;
/* K·H */ /* K·H */
kf->mat_status = KF_MatMult(&kf->K, &kf->H, &kf->temp_matrix); kf->mat_status = KF_MatMult(&kf->K, &kf->H, &kf->temp_matrix);
/* K·H·P'(k) */ /* K·H·P'(k) */
kf->mat_status = KF_MatMult(&kf->temp_matrix, &kf->Pminus, &kf->temp_matrix1); kf->mat_status = KF_MatMult(&kf->temp_matrix, &kf->Pminus, &kf->temp_matrix1);
/* P = P' - K·H·P' */ /* P = P' - K·H·P' */
kf->mat_status = KF_MatSub(&kf->Pminus, &kf->temp_matrix1, &kf->P); kf->mat_status = KF_MatSub(&kf->Pminus, &kf->temp_matrix1, &kf->P);
} }
@ -461,70 +461,70 @@ int8_t KF_UpdateCovariance(KF_t *kf) {
} }
/** /**
* @brief 执行完整的卡尔曼滤波周期（五大方程） * @brief 执行完整的卡尔曼滤波周期（五大方程）
* *
* 实现标准KF，并支持用户自定义函数钩子用于扩展（EKF/UKF/ESKF/AUKF）。 * 实现标准KF，并支持用户自定义函数钩子用于扩展（EKF/UKF/ESKF/AUKF）。
* 每个步骤都可以通过 User_Func 回调函数替换。 * 每个步骤都可以通过 User_Func 回调函数替换。
* *
* @param kf 卡尔曼滤波器结构体指针 * @param kf 卡尔曼滤波器结构体指针
* @return float* 滤波后的状态估计值指针 * @return float* 滤波后的状态估计值指针
*/ */
float *KF_Update(KF_t *kf) { float *KF_Update(KF_t *kf) {
if (kf == NULL) return NULL; if (kf == NULL) return NULL;
/* 步骤0: 量测获取和矩阵调整 */ /* 步骤0: 量测获取和矩阵调整 */
KF_Measure(kf); KF_Measure(kf);
if (kf->User_Func0_f != NULL) kf->User_Func0_f(kf); if (kf->User_Func0_f != NULL) kf->User_Func0_f(kf);
/* 步骤1: 先验状态估计 - xhat'(k) = F·xhat(k-1) + B·u */ /* 步骤1: 先验状态估计 - xhat'(k) = F·xhat(k-1) + B·u */
KF_PredictState(kf); KF_PredictState(kf);
if (kf->User_Func1_f != NULL) kf->User_Func1_f(kf); if (kf->User_Func1_f != NULL) kf->User_Func1_f(kf);
/* 步骤2: 先验协方差 - P'(k) = F·P(k-1)·F^T + Q */ /* 步骤2: 先验协方差 - P'(k) = F·P(k-1)·F^T + Q */
KF_PredictCovariance(kf); KF_PredictCovariance(kf);
if (kf->User_Func2_f != NULL) kf->User_Func2_f(kf); if (kf->User_Func2_f != NULL) kf->User_Func2_f(kf);
/* 量测更新（仅在存在有效量测时执行） */ /* 量测更新（仅在存在有效量测时执行） */
if (kf->measurement_valid_num != 0 || kf->use_auto_adjustment == 0) { if (kf->measurement_valid_num != 0 || kf->use_auto_adjustment == 0) {
/* 步骤3: 卡尔曼增益 - K = P'(k)·H^T / (H·P'(k)·H^T + R) */ /* 步骤3: 卡尔曼增益 - K = P'(k)·H^T / (H·P'(k)·H^T + R) */
KF_CalcGain(kf); KF_CalcGain(kf);
if (kf->User_Func3_f != NULL) kf->User_Func3_f(kf); if (kf->User_Func3_f != NULL) kf->User_Func3_f(kf);
/* 步骤4: 状态更新 - xhat(k) = xhat'(k) + K·(z - H·xhat'(k)) */ /* 步骤4: 状态更新 - xhat(k) = xhat'(k) + K·(z - H·xhat'(k)) */
KF_UpdateState(kf); KF_UpdateState(kf);
if (kf->User_Func4_f != NULL) kf->User_Func4_f(kf); if (kf->User_Func4_f != NULL) kf->User_Func4_f(kf);
/* 步骤5: 协方差更新 - P(k) = P'(k) - K·H·P'(k) */ /* 步骤5: 协方差更新 - P(k) = P'(k) - K·H·P'(k) */
KF_UpdateCovariance(kf); KF_UpdateCovariance(kf);
} else { } else {
/* 无有效量测 - 仅预测 */ /* 无有效量测 - 仅预测 */
memcpy(kf->xhat_data, kf->xhatminus_data, sizeof(float) * kf->xhat_size); memcpy(kf->xhat_data, kf->xhatminus_data, sizeof(float) * kf->xhat_size);
memcpy(kf->P_data, kf->Pminus_data, memcpy(kf->P_data, kf->Pminus_data,
sizeof(float) * kf->xhat_size * kf->xhat_size); sizeof(float) * kf->xhat_size * kf->xhat_size);
} }
/* 后处理钩子 */ /* 后处理钩子 */
if (kf->User_Func5_f != NULL) kf->User_Func5_f(kf); if (kf->User_Func5_f != NULL) kf->User_Func5_f(kf);
/* 防过度收敛：强制最小方差 */ /* 防过度收敛：强制最小方差 */
for (uint8_t i = 0; i < kf->xhat_size; i++) { for (uint8_t i = 0; i < kf->xhat_size; i++) {
if (kf->P_data[i * kf->xhat_size + i] < kf->state_min_variance[i]) if (kf->P_data[i * kf->xhat_size + i] < kf->state_min_variance[i])
kf->P_data[i * kf->xhat_size + i] = kf->state_min_variance[i]; kf->P_data[i * kf->xhat_size + i] = kf->state_min_variance[i];
} }
/* 复制结果到输出 */ /* 复制结果到输出 */
memcpy(kf->filtered_value, kf->xhat_data, sizeof(float) * kf->xhat_size); memcpy(kf->filtered_value, kf->xhat_data, sizeof(float) * kf->xhat_size);
/* 附加后处理钩子 */ /* 附加后处理钩子 */
if (kf->User_Func6_f != NULL) kf->User_Func6_f(kf); if (kf->User_Func6_f != NULL) kf->User_Func6_f(kf);
return kf->filtered_value; return kf->filtered_value;
} }
/** /**
* @brief 重置卡尔曼滤波器状态 * @brief 重置卡尔曼滤波器状态
* *
* @param kf 卡尔曼滤波器结构体指针 * @param kf 卡尔曼滤波器结构体指针
*/ */
void KF_Reset(KF_t *kf) { void KF_Reset(KF_t *kf) {
if (kf == NULL) return; if (kf == NULL) return;
@ -536,45 +536,45 @@ void KF_Reset(KF_t *kf) {
} }
/** /**
* @brief 根据量测有效性动态调整 H, R, K 矩阵 * @brief 根据量测有效性动态调整 H, R, K 矩阵
* *
* 该函数根据当前周期中哪些量测有效（非零）来重建量测相关矩阵。 * 该函数根据当前周期中哪些量测有效（非零）来重建量测相关矩阵。
* 支持具有不同采样率的异步传感器。 * 支持具有不同采样率的异步传感器。
* *
* @param kf 卡尔曼滤波器结构体指针 * @param kf 卡尔曼滤波器结构体指针
*/ */
static void KF_AdjustHKR(KF_t *kf) { static void KF_AdjustHKR(KF_t *kf) {
kf->measurement_valid_num = 0; kf->measurement_valid_num = 0;
/* 复制并重置量测向量 */ /* 复制并重置量测向量 */
memcpy(kf->z_data, kf->measured_vector, sizeof(float) * kf->z_size); memcpy(kf->z_data, kf->measured_vector, sizeof(float) * kf->z_size);
memset(kf->measured_vector, 0, sizeof(float) * kf->z_size); memset(kf->measured_vector, 0, sizeof(float) * kf->z_size);
/* 清空 H 和 R 矩阵 */ /* 清空 H 和 R 矩阵 */
memset(kf->R_data, 0, sizeof(float) * kf->z_size * kf->z_size); memset(kf->R_data, 0, sizeof(float) * kf->z_size * kf->z_size);
memset(kf->H_data, 0, sizeof(float) * kf->xhat_size * kf->z_size); memset(kf->H_data, 0, sizeof(float) * kf->xhat_size * kf->z_size);
/* 识别有效量测并重建 z, H */ /* 识别有效量测并重建 z, H */
for (uint8_t i = 0; i < kf->z_size; i++) { for (uint8_t i = 0; i < kf->z_size; i++) {
if (kf->z_data[i] != 0) { /* 非零表示有效量测 */ if (kf->z_data[i] != 0) { /* 非零表示有效量测 */
/* 将有效量测压缩到 z */ /* 将有效量测压缩到 z */
kf->z_data[kf->measurement_valid_num] = kf->z_data[i]; kf->z_data[kf->measurement_valid_num] = kf->z_data[i];
kf->temp[kf->measurement_valid_num] = i; kf->temp[kf->measurement_valid_num] = i;
/* 重建 H 矩阵行 */ /* 重建 H 矩阵行 */
kf->H_data[kf->xhat_size * kf->measurement_valid_num + kf->H_data[kf->xhat_size * kf->measurement_valid_num +
kf->measurement_map[i] - 1] = kf->measurement_degree[i]; kf->measurement_map[i] - 1] = kf->measurement_degree[i];
kf->measurement_valid_num++; kf->measurement_valid_num++;
} }
} }
/* 用有效量测方差重建 R 矩阵 */ /* 用有效量测方差重建 R 矩阵 */
for (uint8_t i = 0; i < kf->measurement_valid_num; i++) { for (uint8_t i = 0; i < kf->measurement_valid_num; i++) {
kf->R_data[i * kf->measurement_valid_num + i] = kf->R_data[i * kf->measurement_valid_num + i] =
kf->mat_r_diagonal_elements[kf->temp[i]]; kf->mat_r_diagonal_elements[kf->temp[i]];
} }
/* 调整矩阵维度以匹配有效量测数量 */ /* 调整矩阵维度以匹配有效量测数量 */
kf->H.numRows = kf->measurement_valid_num; kf->H.numRows = kf->measurement_valid_num;
kf->H.numCols = kf->xhat_size; kf->H.numCols = kf->xhat_size;
kf->HT.numRows = kf->xhat_size; kf->HT.numRows = kf->xhat_size;