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