AimBot/Gimbal
14
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

第一次提交

Browse Source
This commit is contained in:
李傲琦
2026-05-29 17:36:34 +08:00
commit 5b3333125d
1318 changed files with 722705 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
modules/algorithm/QuaternionEKF.c
+489
View File
@@ -0,0 +1,489 @@
/**
******************************************************************************
* @file QuaternionEKF.c
* @author Wang Hongxi
* @version V1.2.0
* @date 2022/3/8
* @brief attitude update with gyro bias estimate and chi-square test
******************************************************************************
* @attention
* 1st order LPF transfer function:
* 1
* ———————
* as + 1
*
******************************************************************************
*/
#include "QuaternionEKF.h"
QEKF_INS_t QEKF_INS;
const float IMU_QuaternionEKF_F[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};
float IMU_QuaternionEKF_P[36] = {100000, 0.1, 0.1, 0.1, 0.1, 0.1,
0.1, 100000, 0.1, 0.1, 0.1, 0.1,
0.1, 0.1, 100000, 0.1, 0.1, 0.1,
0.1, 0.1, 0.1, 100000, 0.1, 0.1,
0.1, 0.1, 0.1, 0.1, 100, 0.1,
0.1, 0.1, 0.1, 0.1, 0.1, 100};
float IMU_QuaternionEKF_K[18];
float IMU_QuaternionEKF_H[18];
static float invSqrt(float x);
static void IMU_QuaternionEKF_Observe(KalmanFilter_t *kf);
static void IMU_QuaternionEKF_F_Linearization_P_Fading(KalmanFilter_t *kf);
static void IMU_QuaternionEKF_SetH(KalmanFilter_t *kf);
static void IMU_QuaternionEKF_xhatUpdate(KalmanFilter_t *kf);
/**
* @brief Quaternion EKF initialization and some reference value
* @param[in] process_noise1 quaternion process noise 10
* @param[in] process_noise2 gyro bias process noise 0.001
* @param[in] measure_noise accel measure noise 1000000
* @param[in] lambda fading coefficient 0.9996
* @param[in] lpf lowpass filter coefficient 0
*/
void IMU_QuaternionEKF_Init(float* init_quaternion,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.ChiSquareTestThreshold = 1e-8;
QEKF_INS.ConvergeFlag = 0;
QEKF_INS.ErrorCount = 0;
QEKF_INS.UpdateCount = 0;
if (lambda > 1)
{
lambda = 1;
}
QEKF_INS.lambda = lambda;
QEKF_INS.accLPFcoef = lpf;
// 初始化矩阵维度信息
Kalman_Filter_Init(&QEKF_INS.IMU_QuaternionEKF, 6, 0, 3);
Matrix_Init(&QEKF_INS.ChiSquare, 1, 1, (float *)QEKF_INS.ChiSquare_Data);
// 姿态初始化
for(int i = 0; i < 4; i++)
{
QEKF_INS.IMU_QuaternionEKF.xhat_data[i] = init_quaternion[i];
}
// 自定义函数初始化,用于扩展或增加kf的基础功能
QEKF_INS.IMU_QuaternionEKF.User_Func0_f = IMU_QuaternionEKF_Observe;
QEKF_INS.IMU_QuaternionEKF.User_Func1_f = IMU_QuaternionEKF_F_Linearization_P_Fading;
QEKF_INS.IMU_QuaternionEKF.User_Func2_f = IMU_QuaternionEKF_SetH;
QEKF_INS.IMU_QuaternionEKF.User_Func3_f = IMU_QuaternionEKF_xhatUpdate;
// 设定标志位,用自定函数替换kf标准步骤中的SetK(计算增益)以及xhatupdate(后验估计/融合)
QEKF_INS.IMU_QuaternionEKF.SkipEq3 = TRUE;
QEKF_INS.IMU_QuaternionEKF.SkipEq4 = TRUE;
memcpy(QEKF_INS.IMU_QuaternionEKF.F_data, IMU_QuaternionEKF_F, sizeof(IMU_QuaternionEKF_F));
memcpy(QEKF_INS.IMU_QuaternionEKF.P_data, IMU_QuaternionEKF_P, sizeof(IMU_QuaternionEKF_P));
}
/**
* @brief Quaternion EKF update
* @param[in] gyro x y z in rad/s
* @param[in] accel x y z in m/s²
* @param[in] update period in s
*/
void IMU_QuaternionEKF_Update(float gx, float gy, float gz, float ax, float ay, float az, float dt)
{
// 0.5(Ohm-Ohm^bias)*deltaT,用于更新工作点处的状态转移F矩阵
static float halfgxdt, halfgydt, halfgzdt;
static float accelInvNorm;
/* F, number with * represent vals to be set
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.GyroBias[0];
QEKF_INS.Gyro[1] = gy - QEKF_INS.GyroBias[1];
QEKF_INS.Gyro[2] = gz - QEKF_INS.GyroBias[2];
// set 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用单位阵覆盖前一轮线性化后的矩阵
memcpy(QEKF_INS.IMU_QuaternionEKF.F_data, IMU_QuaternionEKF_F, sizeof(IMU_QuaternionEKF_F));
QEKF_INS.IMU_QuaternionEKF.F_data[1] = -halfgxdt;
QEKF_INS.IMU_QuaternionEKF.F_data[2] = -halfgydt;
QEKF_INS.IMU_QuaternionEKF.F_data[3] = -halfgzdt;
QEKF_INS.IMU_QuaternionEKF.F_data[6] = halfgxdt;
QEKF_INS.IMU_QuaternionEKF.F_data[8] = halfgzdt;
QEKF_INS.IMU_QuaternionEKF.F_data[9] = -halfgydt;
QEKF_INS.IMU_QuaternionEKF.F_data[12] = halfgydt;
QEKF_INS.IMU_QuaternionEKF.F_data[13] = -halfgzdt;
QEKF_INS.IMU_QuaternionEKF.F_data[15] = halfgxdt;
QEKF_INS.IMU_QuaternionEKF.F_data[18] = halfgzdt;
QEKF_INS.IMU_QuaternionEKF.F_data[19] = halfgydt;
QEKF_INS.IMU_QuaternionEKF.F_data[20] = -halfgxdt;
// accel low pass filter,加速度过一下低通滤波平滑数据,降低撞击和异常的影响
if (QEKF_INS.UpdateCount == 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.accLPFcoef / (QEKF_INS.dt + QEKF_INS.accLPFcoef) + ax * QEKF_INS.dt / (QEKF_INS.dt + QEKF_INS.accLPFcoef);
QEKF_INS.Accel[1] = QEKF_INS.Accel[1] * QEKF_INS.accLPFcoef / (QEKF_INS.dt + QEKF_INS.accLPFcoef) + ay * QEKF_INS.dt / (QEKF_INS.dt + QEKF_INS.accLPFcoef);
QEKF_INS.Accel[2] = QEKF_INS.Accel[2] * QEKF_INS.accLPFcoef / (QEKF_INS.dt + QEKF_INS.accLPFcoef) + az * QEKF_INS.dt / (QEKF_INS.dt + QEKF_INS.accLPFcoef);
// set z,单位化重力加速度向量
accelInvNorm = 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.IMU_QuaternionEKF.MeasuredVector[i] = QEKF_INS.Accel[i] * accelInvNorm; // 用加速度向量更新量测值
}
// get body state
QEKF_INS.gyro_norm = 1.0f / 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 / accelInvNorm;
// 如果角速度小于阈值且加速度处于设定范围内,认为运动稳定,加速度可以用于修正角速度
// 稍后在最后的姿态更新部分会利用StableFlag来确定
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.StableFlag = 1;
}
else
{
QEKF_INS.StableFlag = 0;
}
// set Q R,过程噪声和观测噪声矩阵
QEKF_INS.IMU_QuaternionEKF.Q_data[0] = QEKF_INS.Q1 * QEKF_INS.dt;
QEKF_INS.IMU_QuaternionEKF.Q_data[7] = QEKF_INS.Q1 * QEKF_INS.dt;
QEKF_INS.IMU_QuaternionEKF.Q_data[14] = QEKF_INS.Q1 * QEKF_INS.dt;
QEKF_INS.IMU_QuaternionEKF.Q_data[21] = QEKF_INS.Q1 * QEKF_INS.dt;
QEKF_INS.IMU_QuaternionEKF.Q_data[28] = QEKF_INS.Q2 * QEKF_INS.dt;
QEKF_INS.IMU_QuaternionEKF.Q_data[35] = QEKF_INS.Q2 * QEKF_INS.dt;
QEKF_INS.IMU_QuaternionEKF.R_data[0] = QEKF_INS.R;
QEKF_INS.IMU_QuaternionEKF.R_data[4] = QEKF_INS.R;
QEKF_INS.IMU_QuaternionEKF.R_data[8] = QEKF_INS.R;
// 调用kalman_filter.c封装好的函数,注意几个User_Funcx_f的调用
Kalman_Filter_Update(&QEKF_INS.IMU_QuaternionEKF);
// 获取融合后的数据,包括四元数和xy零飘值
QEKF_INS.q[0] = QEKF_INS.IMU_QuaternionEKF.FilteredValue[0];
QEKF_INS.q[1] = QEKF_INS.IMU_QuaternionEKF.FilteredValue[1];
QEKF_INS.q[2] = QEKF_INS.IMU_QuaternionEKF.FilteredValue[2];
QEKF_INS.q[3] = QEKF_INS.IMU_QuaternionEKF.FilteredValue[3];
QEKF_INS.GyroBias[0] = QEKF_INS.IMU_QuaternionEKF.FilteredValue[4];
QEKF_INS.GyroBias[1] = QEKF_INS.IMU_QuaternionEKF.FilteredValue[5];
QEKF_INS.GyroBias[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;
// get Yaw total, yaw数据可能会超过360,处理一下方便其他功能使用(如小陀螺)
if (QEKF_INS.Yaw - QEKF_INS.YawAngleLast > 180.0f)
{
QEKF_INS.YawRoundCount--;
}
else if (QEKF_INS.Yaw - QEKF_INS.YawAngleLast < -180.0f)
{
QEKF_INS.YawRoundCount++;
}
QEKF_INS.YawTotalAngle = 360.0f * QEKF_INS.YawRoundCount + QEKF_INS.Yaw;
QEKF_INS.YawAngleLast = QEKF_INS.Yaw;
QEKF_INS.UpdateCount++; // 初始化低通滤波用,计数测试用
}
/**
* @brief 用于更新线性化后的状态转移矩阵F右上角的一个4x2分块矩阵,稍后用于协方差矩阵P的更新;
* 并对零漂的方差进行限制,防止过度收敛并限幅防止发散
*
* @param kf
*/
static void IMU_QuaternionEKF_F_Linearization_P_Fading(KalmanFilter_t *kf)
{
static float q0, q1, q2, q3;
static float qInvNorm;
q0 = kf->xhatminus_data[0];
q1 = kf->xhatminus_data[1];
q2 = kf->xhatminus_data[2];
q3 = kf->xhatminus_data[3];
// quaternion normalize
qInvNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
for (uint8_t i = 0; i < 4; ++i)
{
kf->xhatminus_data[i] *= qInvNorm;
}
/* F, number with * represent vals to be set
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
*/
// set F
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;
// fading filter,防止零飘参数过度收敛
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 IMU_QuaternionEKF_SetH(KalmanFilter_t *kf)
{
static float doubleq0, doubleq1, doubleq2, doubleq3;
/* H
0 1 2 3 4 5
6 7 8 9 10 11
12 13 14 15 16 17
last two cols are zero
*/
// set H
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->zSize * kf->xhatSize);
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 IMU_QuaternionEKF_xhatUpdate(KalmanFilter_t *kf)
{
static float q0, q1, q2, q3;
kf->MatStatus = Matrix_Transpose(&kf->H, &kf->HT); // z|x => x|z
kf->temp_matrix.numRows = kf->H.numRows;
kf->temp_matrix.numCols = kf->Pminus.numCols;
kf->MatStatus = Matrix_Multiply(&kf->H, &kf->Pminus, &kf->temp_matrix); // temp_matrix = H·P'(k)
kf->temp_matrix1.numRows = kf->temp_matrix.numRows;
kf->temp_matrix1.numCols = kf->HT.numCols;
kf->MatStatus = Matrix_Multiply(&kf->temp_matrix, &kf->HT, &kf->temp_matrix1); // temp_matrix1 = H·P'(k)·HT
kf->S.numRows = kf->R.numRows;
kf->S.numCols = kf->R.numCols;
kf->MatStatus = Matrix_Add(&kf->temp_matrix1, &kf->R, &kf->S); // S = H P'(k) HT + R
kf->MatStatus = Matrix_Inverse(&kf->S, &kf->temp_matrix1); // temp_matrix1 = inv(H·P'(k)·HT + R)
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);
kf->temp_vector_data[2] = q0 * q0 - q1 * q1 - q2 * q2 + q3 * q3; // temp_vector = h(xhat'(k))
// 计算预测值和各个轴的方向余弦
for (uint8_t i = 0; i < 3; ++i)
{
QEKF_INS.OrientationCosine[i] = acosf(fabsf(kf->temp_vector_data[i]));
}
// 利用加速度计数据修正
kf->temp_vector1.numRows = kf->z.numRows;
kf->temp_vector1.numCols = 1;
kf->MatStatus = Matrix_Subtract(&kf->z, &kf->temp_vector, &kf->temp_vector1); // temp_vector1 = z(k) - h(xhat'(k))
// chi-square test,卡方检验
kf->temp_matrix.numRows = kf->temp_vector1.numRows;
kf->temp_matrix.numCols = 1;
kf->MatStatus = Matrix_Multiply(&kf->temp_matrix1, &kf->temp_vector1, &kf->temp_matrix); // temp_matrix = inv(H·P'(k)·HT + R)·(z(k) - h(xhat'(k)))
kf->temp_vector.numRows = 1;
kf->temp_vector.numCols = kf->temp_vector1.numRows;
kf->MatStatus = Matrix_Transpose(&kf->temp_vector1, &kf->temp_vector); // temp_vector = z(k) - h(xhat'(k))'
kf->MatStatus = Matrix_Multiply(&kf->temp_vector, &kf->temp_matrix, &QEKF_INS.ChiSquare);
// rk is small,filter converged/converging
if (QEKF_INS.ChiSquare_Data[0] < 0.5f * QEKF_INS.ChiSquareTestThreshold)
{
QEKF_INS.ConvergeFlag = 1;
}
// rk is bigger than thre but once converged
if (QEKF_INS.ChiSquare_Data[0] > QEKF_INS.ChiSquareTestThreshold && QEKF_INS.ConvergeFlag)
{
if (QEKF_INS.StableFlag)
{
QEKF_INS.ErrorCount++; // 载体静止时仍无法通过卡方检验
}
else
{
QEKF_INS.ErrorCount = 0;
}
if (QEKF_INS.ErrorCount > 50)
{
// 滤波器发散
QEKF_INS.ConvergeFlag = 0;
kf->SkipEq5 = FALSE; // step-5 is cov mat P updating
}
else
{
// 残差未通过卡方检验 仅预测
// xhat(k) = xhat'(k)
// P(k) = P'(k)
memcpy(kf->xhat_data, kf->xhatminus_data, sizeof_float * kf->xhatSize);
memcpy(kf->P_data, kf->Pminus_data, sizeof_float * kf->xhatSize * kf->xhatSize);
kf->SkipEq5 = TRUE; // part5 is P updating
return;
}
}
else // if divergent or rk is not that big/acceptable,use adaptive gain
{
// scale adaptive,rk越小则增益越大,否则更相信预测值
if (QEKF_INS.ChiSquare_Data[0] > 0.1f * QEKF_INS.ChiSquareTestThreshold && QEKF_INS.ConvergeFlag)
{
QEKF_INS.AdaptiveGainScale = (QEKF_INS.ChiSquareTestThreshold - QEKF_INS.ChiSquare_Data[0]) / (0.9f * QEKF_INS.ChiSquareTestThreshold);
}
else
{
QEKF_INS.AdaptiveGainScale = 1;
}
QEKF_INS.ErrorCount = 0;
kf->SkipEq5 = FALSE;
}
// cal kf-gain K
kf->temp_matrix.numRows = kf->Pminus.numRows;
kf->temp_matrix.numCols = kf->HT.numCols;
kf->MatStatus = Matrix_Multiply(&kf->Pminus, &kf->HT, &kf->temp_matrix); // temp_matrix = P'(k)·HT
kf->MatStatus = Matrix_Multiply(&kf->temp_matrix, &kf->temp_matrix1, &kf->K);
// implement adaptive
for (uint8_t i = 0; i < kf->K.numRows * kf->K.numCols; ++i)
{
kf->K_data[i] *= QEKF_INS.AdaptiveGainScale;
}
for (uint8_t i = 4; i < 6; ++i)
{
for (uint8_t j = 0; j < 3; ++j)
{
kf->K_data[i * 3 + j] *= QEKF_INS.OrientationCosine[i - 4] / 1.5707963f; // 1 rad
}
}
kf->temp_vector.numRows = kf->K.numRows;
kf->temp_vector.numCols = 1;
kf->MatStatus = Matrix_Multiply(&kf->K, &kf->temp_vector1, &kf->temp_vector); // temp_vector = K(k)·(z(k) - H·xhat'(k))
// 零漂修正限幅,一般不会有过大的漂移
if (QEKF_INS.ConvergeFlag)
{
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->MatStatus = Matrix_Add(&kf->xhatminus, &kf->temp_vector, &kf->xhat);
}
/**
* @brief EKF观测环节,其实就是把数据复制一下
*
* @param kf kf类型定义
*/
static void IMU_QuaternionEKF_Observe(KalmanFilter_t *kf)
{
memcpy(IMU_QuaternionEKF_P, kf->P_data, sizeof(IMU_QuaternionEKF_P));
memcpy(IMU_QuaternionEKF_K, kf->K_data, sizeof(IMU_QuaternionEKF_K));
memcpy(IMU_QuaternionEKF_H, kf->H_data, sizeof(IMU_QuaternionEKF_H));
}
/**
* @brief 自定义1/sqrt(x),速度更快
*
* @param x x
* @return float
*/
static float 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;
}
modules/algorithm/QuaternionEKF.h
+75
View File
@@ -0,0 +1,75 @@
/**
******************************************************************************
* @file QuaternionEKF.h
* @author Wang Hongxi
* @version V1.2.0
* @date 2022/3/8
* @brief attitude update with gyro bias estimate and chi-square test
******************************************************************************
* @attention
*
******************************************************************************
*/
#ifndef _QUAT_EKF_H
#define _QUAT_EKF_H
#include "kalman_filter.h"
/* boolean type definitions */
#ifndef TRUE
#define TRUE 1 /**< boolean true */
#endif
#ifndef FALSE
#define FALSE 0 /**< boolean fails */
#endif
typedef struct
{
uint8_t Initialized;
KalmanFilter_t IMU_QuaternionEKF;
uint8_t ConvergeFlag;
uint8_t StableFlag;
uint64_t ErrorCount;
uint64_t UpdateCount;
float q[4]; // 四元数估计值
float GyroBias[3]; // 陀螺仪零偏估计值
float Gyro[3];
float Accel[3];
float OrientationCosine[3];
float accLPFcoef;
float gyro_norm;
float accl_norm;
float AdaptiveGainScale;
float Roll;
float Pitch;
float Yaw;
float YawTotalAngle;
float Q1; // 四元数更新过程噪声
float Q2; // 陀螺仪零偏过程噪声
float R; // 加速度计量测噪声
float dt; // 姿态更新周期
mat ChiSquare;
float ChiSquare_Data[1]; // 卡方检验检测函数
float ChiSquareTestThreshold; // 卡方检验阈值
float lambda; // 渐消因子
int16_t YawRoundCount;
float YawAngleLast;
} QEKF_INS_t;
extern QEKF_INS_t QEKF_INS;
extern float chiSquare;
extern float ChiSquareTestThreshold;
void IMU_QuaternionEKF_Init(float* init_quaternion,float process_noise1, float process_noise2, float measure_noise, float lambda, float lpf);
void IMU_QuaternionEKF_Update(float gx, float gy, float gz, float ax, float ay, float az, float dt);
#endif
modules/algorithm/algorithm.md
+29
View File
@@ -0,0 +1,29 @@
# algorithms
<p align='right'>neozng1@hnu.edu.cn</p>
> TODO:
> 1. 实现麦轮和全向轮的速度解算
> 2. 实现一些通用的滤波器，如指数平均，窗平均，低通等
## 总览和使用
module层的algorithm提供了一些供其他模块以及app的应用层使用的算法，包括：
1. PID控制器`controller.h`
2. crc8 crc16循环冗余校验
3. 卡尔曼滤波器`kalman_filter.h`，可以通过用户自定义函数配置为扩展卡尔曼滤波
4. `LQR.h`，线性二次型调节器
5. `QuaterninoEKF.h`，用于`ins_task`的四元数姿态解算和扩展卡尔曼滤波融合
6. `user_lib.h`，一些通用的函数，包括限幅、数据类型转换、角度弧度转换、快速符号判断以及优化开方等功能。多个模块都会使用的、不好区分的函数可以放置于此
## 代码结构
.c 为算法的实现，.h为算法对外接口的头文件
在编写应用的时候，你基本不会使用这里的函数，或是修改其实现。
若发现bug或需要增加功能，联系组长讨论。
modules/algorithm/controller.c
+211
View File
@@ -0,0 +1,211 @@
/**
* @file controller.c
* @author wanghongxi
* @author modified by neozng
* @brief PID控制器定义
* @version beta
* @date 2022-11-01
*
* @copyrightCopyright (c) 2022 HNU YueLu EC all rights reserved
*/
#include "controller.h"
#include "memory.h"
/* ----------------------------下面是pid优化环节的实现---------------------------- */
// 梯形积分
static void f_Trapezoid_Intergral(PIDInstance *pid)
{
// 计算梯形的面积,(上底+下底)*高/2
pid->ITerm = pid->Ki * ((pid->Err + pid->Last_Err) / 2) * pid->dt;
}
// 变速积分(误差小时积分作用更强)
static void f_Changing_Integration_Rate(PIDInstance *pid)
{
if (pid->Err * pid->Iout > 0)
{
// 积分呈累积趋势
if (abs(pid->Err) <= pid->CoefB)
return; // Full integral
if (abs(pid->Err) <= (pid->CoefA + pid->CoefB))
pid->ITerm *= (pid->CoefA - abs(pid->Err) + pid->CoefB) / pid->CoefA;
else // 最大阈值,不使用积分
pid->ITerm = 0;
}
}
static void f_Integral_Limit(PIDInstance *pid)
{
static float temp_Output, temp_Iout;
temp_Iout = pid->Iout + pid->ITerm;
temp_Output = pid->Pout + pid->Iout + pid->Dout;
if (abs(temp_Output) > pid->MaxOut)
{
if (pid->Err * pid->Iout > 0) // 积分却还在累积
{
pid->ITerm = 0; // 当前积分项置零
}
}
if (temp_Iout > pid->IntegralLimit)
{
pid->ITerm = 0;
pid->Iout = pid->IntegralLimit;
}
if (temp_Iout < -pid->IntegralLimit)
{
pid->ITerm = 0;
pid->Iout = -pid->IntegralLimit;
}
}
// 微分先行(仅使用反馈值而不计参考输入的微分)
static void f_Derivative_On_Measurement(PIDInstance *pid)
{
pid->Dout = pid->Kd * (pid->Last_Measure - pid->Measure) / pid->dt;
}
// 微分滤波(采集微分时,滤除高频噪声)
static void f_Derivative_Filter(PIDInstance *pid)
{
pid->Dout = pid->Dout * pid->dt / (pid->Derivative_LPF_RC + pid->dt) +
pid->Last_Dout * pid->Derivative_LPF_RC / (pid->Derivative_LPF_RC + pid->dt);
}
// 输出滤波
static void f_Output_Filter(PIDInstance *pid)
{
pid->Output = pid->Output * pid->dt / (pid->Output_LPF_RC + pid->dt) +
pid->Last_Output * pid->Output_LPF_RC / (pid->Output_LPF_RC + pid->dt);
}
// 输出限幅
static void f_Output_Limit(PIDInstance *pid)
{
if (pid->Output > pid->MaxOut)
{
pid->Output = pid->MaxOut;
}
if (pid->Output < -(pid->MaxOut))
{
pid->Output = -(pid->MaxOut);
}
}
// 电机堵转检测
static void f_PID_ErrorHandle(PIDInstance *pid)
{
/*Motor Blocked Handle*/
if (fabsf(pid->Output) < pid->MaxOut * 0.001f || fabsf(pid->Ref) < 0.0001f)
return;
if ((fabsf(pid->Ref - pid->Measure) / fabsf(pid->Ref)) > 0.95f)
{
// Motor blocked counting
pid->ERRORHandler.ERRORCount++;
}
else
{
pid->ERRORHandler.ERRORCount = 0;
}
if (pid->ERRORHandler.ERRORCount > 500)
{
// Motor blocked over 1000times
pid->ERRORHandler.ERRORType = PID_MOTOR_BLOCKED_ERROR;
}
}
/* ---------------------------下面是PID的外部算法接口--------------------------- */
/**
* @brief 初始化PID,设置参数和启用的优化环节,将其他数据置零
*
* @param pid PID实例
* @param config PID初始化设置
*/
void PIDInit(PIDInstance *pid, PID_Init_Config_s *config)
{
// config的数据和pid的部分数据是连续且相同的的,所以可以直接用memcpy
// @todo: 不建议这样做,可扩展性差,不知道的开发者可能会误以为pid和config是同一个结构体
// 后续修改为逐个赋值
memset(pid, 0, sizeof(PIDInstance));
// utilize the quality of struct that its memeory is continuous
memcpy(pid, config, sizeof(PID_Init_Config_s));
// set rest of memory to 0
DWT_GetDeltaT(&pid->DWT_CNT);
}
/**
* @brief PID计算
* @param[in] PID结构体
* @param[in] 测量值
* @param[in] 期望值
* @retval 返回空
*/
float PIDCalculate(PIDInstance *pid, float measure, float ref)
{
// 堵转检测
if (pid->Improve & PID_ErrorHandle)
f_PID_ErrorHandle(pid);
pid->dt = DWT_GetDeltaT(&pid->DWT_CNT); // 获取两次pid计算的时间间隔,用于积分和微分
if (pid->dt < 0.000001f) // 避免除零
{
return 0.0f;
}
// 保存上次的测量值和误差,计算当前error
pid->Measure = measure;
pid->Ref = ref;
pid->Err = pid->Ref - pid->Measure;
// 如果在死区外,则计算PID
if (abs(pid->Err) > pid->DeadBand)
{
// 基本的pid计算,使用位置式
pid->Pout = pid->Kp * pid->Err;
pid->ITerm = pid->Ki * pid->Err * pid->dt;
pid->Dout = pid->Kd * (pid->Err - pid->Last_Err) / pid->dt;
// 梯形积分
if (pid->Improve & PID_Trapezoid_Intergral)
f_Trapezoid_Intergral(pid);
// 变速积分
if (pid->Improve & PID_ChangingIntegrationRate)
f_Changing_Integration_Rate(pid);
// 微分先行
if (pid->Improve & PID_Derivative_On_Measurement)
f_Derivative_On_Measurement(pid);
// 微分滤波器
if (pid->Improve & PID_DerivativeFilter)
f_Derivative_Filter(pid);
// 积分限幅
if (pid->Improve & PID_Integral_Limit)
f_Integral_Limit(pid);
pid->Iout += pid->ITerm; // 累加积分
pid->Output = pid->Pout + pid->Iout + pid->Dout; // 计算输出
// 输出滤波
if (pid->Improve & PID_OutputFilter)
f_Output_Filter(pid);
// 输出限幅
f_Output_Limit(pid);
}
else // 进入死区, 则清空积分和输出
{
pid->Output = 0;
pid->ITerm = 0;
}
// 保存当前数据,用于下次计算
pid->Last_Measure = pid->Measure;
pid->Last_Output = pid->Output;
pid->Last_Dout = pid->Dout;
pid->Last_Err = pid->Err;
pid->Last_ITerm = pid->ITerm;
return pid->Output;
}
modules/algorithm/controller.h
+136
View File
@@ -0,0 +1,136 @@
/**
******************************************************************************
* @file controller.h
* @author Wang Hongxi
* @version V1.1.3
* @date 2021/7/3
* @brief
******************************************************************************
* @attention
*
******************************************************************************
*/
#ifndef _CONTROLLER_H
#define _CONTROLLER_H
#include "main.h"
#include "stdint.h"
#include "memory.h"
#include "stdlib.h"
#include "bsp_dwt.h"
#include "arm_math.h"
#include <math.h>
#ifndef abs
#define abs(x) ((x > 0) ? x : -x)
#endif
// PID 优化环节使能标志位,通过位与可以判断启用的优化环节;也可以改成位域的形式
typedef enum
{
PID_IMPROVE_NONE = 0b00000000, // 0000 0000
PID_Integral_Limit = 0b00000001, // 0000 0001
PID_Derivative_On_Measurement = 0b00000010, // 0000 0010
PID_Trapezoid_Intergral = 0b00000100, // 0000 0100
PID_Proportional_On_Measurement = 0b00001000, // 0000 1000
PID_OutputFilter = 0b00010000, // 0001 0000
PID_ChangingIntegrationRate = 0b00100000, // 0010 0000
PID_DerivativeFilter = 0b01000000, // 0100 0000
PID_ErrorHandle = 0b10000000, // 1000 0000
} PID_Improvement_e;
/* PID 报错类型枚举*/
typedef enum errorType_e
{
PID_ERROR_NONE = 0x00U,
PID_MOTOR_BLOCKED_ERROR = 0x01U
} ErrorType_e;
typedef struct
{
uint64_t ERRORCount;
ErrorType_e ERRORType;
} PID_ErrorHandler_t;
/* PID结构体 */
typedef struct
{
//---------------------------------- init config block
// config parameter
float Kp;
float Ki;
float Kd;
float MaxOut;
float DeadBand;
// improve parameter
PID_Improvement_e Improve;
float IntegralLimit; // 积分限幅
float CoefA; // 变速积分 For Changing Integral
float CoefB; // 变速积分 ITerm = Err*((A-abs(err)+B)/A) when B<|err|<A+B
float Output_LPF_RC; // 输出滤波器 RC = 1/omegac
float Derivative_LPF_RC; // 微分滤波器系数
//-----------------------------------
// for calculating
float Measure;
float Last_Measure;
float Err;
float Last_Err;
float Last_ITerm;
float Pout;
float Iout;
float Dout;
float ITerm;
float Output;
float Last_Output;
float Last_Dout;
float Ref;
uint32_t DWT_CNT;
float dt;
PID_ErrorHandler_t ERRORHandler;
} PIDInstance;
/* 用于PID初始化的结构体*/
typedef struct // config parameter
{
// basic parameter
float Kp;
float Ki;
float Kd;
float MaxOut; // 输出限幅
float DeadBand; // 死区
// improve parameter
PID_Improvement_e Improve;
float IntegralLimit; // 积分限幅
float CoefA; // AB为变速积分参数,变速积分实际上就引入了积分分离
float CoefB; // ITerm = Err*((A-abs(err)+B)/A) when B<|err|<A+B
float Output_LPF_RC; // RC = 1/omegac
float Derivative_LPF_RC;
} PID_Init_Config_s;
/**
* @brief 初始化PID实例
* @todo 待修改为统一的PIDRegister风格
* @param pid PID实例指针
* @param config PID初始化配置
*/
void PIDInit(PIDInstance *pid, PID_Init_Config_s *config);
/**
* @brief 计算PID输出
*
* @param pid PID实例指针
* @param measure 反馈值
* @param ref 设定值
* @return float PID计算输出
*/
float PIDCalculate(PIDInstance *pid, float measure, float ref);
#endif
modules/algorithm/crc16.c
+101
View File
@@ -0,0 +1,101 @@
#include "crc16.h"
static uint8_t crc_tab16_init = 0;
static uint16_t crc_tab16[256];
/*
* uint16_t crc_16( const unsigned char *input_str, size_t num_bytes );
*
*函数crc_16()一次计算一个字节的16位CRC16
*其开头已传递给函数的字符串。的数量
*要检查的字节也是一个参数。字符串中的字节数为
*受恒定大小最大值的限制。
*/
uint16_t crc_16(const uint8_t *input_str, uint16_t num_bytes)
{
uint16_t crc;
const uint8_t *ptr;
uint16_t a;
if (!crc_tab16_init)
init_crc16_tab();
crc = CRC_START_16;
ptr = input_str;
if (ptr != NULL)
for (a = 0; a < num_bytes; a++)
{
crc = (crc >> 8) ^ crc_tab16[(crc ^ (uint16_t)*ptr++) & 0x00FF];
}
return crc;
}
/*
* uint16_t crc_modbus( const unsigned char *input_str, size_t num_bytes );
*
*函数crc_modbus()一次计算16位modbus循环冗余校验
*一个字节字符串，其开头已被传递给函数。这
*要检查的字节数也是一个参数。
*/
uint16_t crc_modbus(const uint8_t *input_str, uint16_t num_bytes)
{
uint16_t crc;
const uint8_t *ptr;
uint16_t a;
if (!crc_tab16_init)
init_crc16_tab();
crc = CRC_START_MODBUS;
ptr = input_str;
if (ptr != NULL)
for (a = 0; a < num_bytes; a++)
{
crc = (crc >> 8) ^ crc_tab16[(crc ^ (uint16_t)*ptr++) & 0x00FF];
}
return crc;
}
/*
* uint16_t update_crc_16( uint16_t crc, unsigned char c );
*
*函数update_crc_16()根据
*前一个循环冗余校验值和下一个要检查的数据字节。
*/
uint16_t update_crc_16(uint16_t crc, uint8_t c)
{
if (!crc_tab16_init)
init_crc16_tab();
return (crc >> 8) ^ crc_tab16[(crc ^ (uint16_t)c) & 0x00FF];
}
/*
* static void init_crc16_tab( void );
*
*为了获得最佳性能，使用CRC16例程查找带有值的表
*可以直接在异或算法中使用的算法。
*查找表首次由init_crc16_tab()例程计算
*调用循环冗余校验函数。
*/
void init_crc16_tab(void)
{
uint16_t i;
uint16_t j;
uint16_t crc;
uint16_t c;
for (i = 0; i < 256; i++)
{
crc = 0;
c = i;
for (j = 0; j < 8; j++)
{
if ((crc ^ c) & 0x0001)
crc = (crc >> 1) ^ CRC_POLY_16;
else
crc = crc >> 1;
c = c >> 1;
}
crc_tab16[i] = crc;
}
crc_tab16_init = 1;
}
modules/algorithm/crc16.h
+14
View File
@@ -0,0 +1,14 @@
#ifndef __CRC16_H
#define __CRC16_H
#include "main.h"
#define CRC_START_16 0xFFFF
#define CRC_START_MODBUS 0xFFFF
#define CRC_POLY_16 0xA001
uint16_t crc_16(const uint8_t *input_str, uint16_t num_bytes);
uint16_t crc_modbus(const uint8_t *input_str, uint16_t num_bytes);
uint16_t update_crc_16(uint16_t crc, uint8_t c);
void init_crc16_tab(void);
#endif
modules/algorithm/crc8.c
+61
View File
@@ -0,0 +1,61 @@
#include "crc8.h"
/*
* static uint8_t sht75_crc_table[];
*
* The SHT75 humidity sensor is capable of calculating an 8 bit CRC checksum to
* ensure data integrity. The lookup table crc_table[] is used to recalculate
* the CRC.
*/
static uint8_t sht75_crc_table[] =
{
0, 49, 98, 83, 196, 245, 166, 151, 185, 136, 219, 234, 125, 76, 31, 46,
67, 114, 33, 16, 135, 182, 229, 212, 250, 203, 152, 169, 62, 15, 92, 109,
134, 183, 228, 213, 66, 115, 32, 17, 63, 14, 93, 108, 251, 202, 153, 168,
197, 244, 167, 150, 1, 48, 99, 82, 124, 77, 30, 47, 184, 137, 218, 235,
61, 12, 95, 110, 249, 200, 155, 170, 132, 181, 230, 215, 64, 113, 34, 19,
126, 79, 28, 45, 186, 139, 216, 233, 199, 246, 165, 148, 3, 50, 97, 80,
187, 138, 217, 232, 127, 78, 29, 44, 2, 51, 96, 81, 198, 247, 164, 149,
248, 201, 154, 171, 60, 13, 94, 111, 65, 112, 35, 18, 133, 180, 231, 214,
122, 75, 24, 41, 190, 143, 220, 237, 195, 242, 161, 144, 7, 54, 101, 84,
57, 8, 91, 106, 253, 204, 159, 174, 128, 177, 226, 211, 68, 117, 38, 23,
252, 205, 158, 175, 56, 9, 90, 107, 69, 116, 39, 22, 129, 176, 227, 210,
191, 142, 221, 236, 123, 74, 25, 40, 6, 55, 100, 85, 194, 243, 160, 145,
71, 118, 37, 20, 131, 178, 225, 208, 254, 207, 156, 173, 58, 11, 88, 105,
4, 53, 102, 87, 192, 241, 162, 147, 189, 140, 223, 238, 121, 72, 27, 42,
193, 240, 163, 146, 5, 52, 103, 86, 120, 73, 26, 43, 188, 141, 222, 239,
130, 179, 224, 209, 70, 119, 36, 21, 59, 10, 89, 104, 255, 206, 157, 172};
/*
* uint8_t crc_8( const unsigned char *input_str, size_t num_bytes );
*
*函数crc_8()计算输入字符串的8位宽crc
*给定长度。
*/
uint8_t crc_8(const uint8_t *input_str, uint16_t num_bytes)
{
uint16_t a;
uint8_t crc;
const uint8_t *ptr;
crc = CRC_START_8;
ptr = input_str;
if (ptr != NULL)
for (a = 0; a < num_bytes; a++)
{
crc = sht75_crc_table[(*ptr++) ^ crc];
}
return crc;
}
/*
* uint8_t update_crc_8( unsigned char crc, unsigned char val );
*
*给定一个数据字节和循环冗余校验值的前一个值
*update_crc_8()计算并返回数据的新的实际crc值
*/
uint8_t update_crc_8(uint8_t crc, uint8_t val)
{
return sht75_crc_table[val ^ crc];
}
modules/algorithm/crc8.h
+11
View File
@@ -0,0 +1,11 @@
#ifndef __CRC8_H
#define __CRC8_H
#include "main.h"
#define CRC_START_8 0x00
uint8_t crc_8(const uint8_t *input_str, uint16_t num_bytes);
uint8_t update_crc_8(uint8_t crc, uint8_t val);
#endif
modules/algorithm/kalman_filter.c
+480
View File
@@ -0,0 +1,480 @@
/**
******************************************************************************
* @file kalman filter.c
* @author Wang Hongxi
* @version V1.2.2
* @date 2022/1/8
* @brief C implementation of kalman filter
******************************************************************************
* @attention
* 该卡尔曼滤波器可以在传感器采样频率不同的情况下，动态调整矩阵H R和K的维数与数值。
* This implementation of kalman filter can dynamically adjust dimension and
* value of matrix H R and K according to the measurement validity under any
* circumstance that the sampling rate of component sensors are different.
*
* 因此矩阵H和R的初始化会与矩阵P A和Q有所不同。另外的，在初始化量测向量z时需要额外写
* 入传感器量测所对应的状态与这个量测的方式，详情请见例程
* Therefore, the initialization of matrix P, F, and Q is sometimes different
* from that of matrices H R. when initialization. Additionally, the corresponding
* state and the method of the measurement should be provided when initializing
* measurement vector z. For more details, please see the example.
*
* 若不需要动态调整量测向量z，可简单将结构体中的Use_Auto_Adjustment初始化为0，并像初
* 始化矩阵P那样用常规方式初始化z H R即可。
* If automatic adjustment is not required, assign zero to the UseAutoAdjustment
* and initialize z H R in the normal way as matrix P.
*
* 要求量测向量z与控制向量u在传感器回调函数中更新。整数0意味着量测无效，即自上次卡尔曼
* 滤波更新后无传感器数据更新。因此量测向量z与控制向量u会在卡尔曼滤波更新过程中被清零
* MeasuredVector and ControlVector are required to be updated in the sensor
* callback function. Integer 0 in measurement vector z indicates the invalidity
* of current measurement, so MeasuredVector and ControlVector will be reset
* (to 0) during each update.
*
* 此外，矩阵P过度收敛后滤波器将难以再适应状态的缓慢变化，从而产生滤波估计偏差。该算法
* 通过限制矩阵P最小值的方法，可有效抑制滤波器的过度收敛，详情请见例程。
* Additionally, the excessive convergence of matrix P will make filter incapable
* of adopting the slowly changing state. This implementation can effectively
* suppress filter excessive convergence through boundary limiting for matrix P.
* For more details, please see the example.
*
* @example:
* x =
* | height |
* | velocity |
* |acceleration|
*
* KalmanFilter_t Height_KF;
*
* void INS_Task_Init(void)
* {
* static float P_Init[9] =
* {
* 10, 0, 0,
* 0, 30, 0,
* 0, 0, 10,
* };
* static float F_Init[9] =
* {
* 1, dt, 0.5*dt*dt,
* 0, 1, dt,
* 0, 0, 1,
* };
* static float Q_Init[9] =
* {
* 0.25*dt*dt*dt*dt, 0.5*dt*dt*dt, 0.5*dt*dt,
* 0.5*dt*dt*dt, dt*dt, dt,
* 0.5*dt*dt, dt, 1,
* };
*
* // 设置最小方差
* static float state_min_variance[3] = {0.03, 0.005, 0.1};
*
* // 开启自动调整
* Height_KF.UseAutoAdjustment = 1;
*
* // 气压测得高度 GPS测得高度 加速度计测得z轴运动加速度
* static uint8_t measurement_reference[3] = {1, 1, 3}
*
* static float measurement_degree[3] = {1, 1, 1}
* // 根据measurement_reference与measurement_degree生成H矩阵如下（在当前周期全部测量数据有效情况下）
* |1 0 0|
* |1 0 0|
* |0 0 1|
*
* static float mat_R_diagonal_elements = {30, 25, 35}
* //根据mat_R_diagonal_elements生成R矩阵如下（在当前周期全部测量数据有效情况下）
* |30 0 0|
* | 0 25 0|
* | 0 0 35|
*
* Kalman_Filter_Init(&Height_KF, 3, 0, 3);
*
* // 设置矩阵值
* memcpy(Height_KF.P_data, P_Init, sizeof(P_Init));
* memcpy(Height_KF.F_data, F_Init, sizeof(F_Init));
* memcpy(Height_KF.Q_data, Q_Init, sizeof(Q_Init));
* memcpy(Height_KF.MeasurementMap, measurement_reference, sizeof(measurement_reference));
* memcpy(Height_KF.MeasurementDegree, measurement_degree, sizeof(measurement_degree));
* memcpy(Height_KF.MatR_DiagonalElements, mat_R_diagonal_elements, sizeof(mat_R_diagonal_elements));
* memcpy(Height_KF.StateMinVariance, state_min_variance, sizeof(state_min_variance));
* }
*
* void INS_Task(void const *pvParameters)
* {
* // 循环更新
* Kalman_Filter_Update(&Height_KF);
* vTaskDelay(ts);
* }
*
* // 测量数据更新应按照以下形式 即向MeasuredVector赋值
* void Barometer_Read_Over(void)
* {
* ......
* INS_KF.MeasuredVector[0] = baro_height;
* }
* void GPS_Read_Over(void)
* {
* ......
* INS_KF.MeasuredVector[1] = GPS_height;
* }
* void Acc_Data_Process(void)
* {
* ......
* INS_KF.MeasuredVector[2] = acc.z;
* }
******************************************************************************
*/
#include "kalman_filter.h"
uint16_t sizeof_float, sizeof_double;
static void H_K_R_Adjustment(KalmanFilter_t *kf);
/**
* @brief 初始化矩阵维度信息并为矩阵分配空间
*
* @param kf kf类型定义
* @param xhatSize 状态变量维度
* @param uSize 控制变量维度
* @param zSize 观测量维度
*/
void Kalman_Filter_Init(KalmanFilter_t *kf, uint8_t xhatSize, uint8_t uSize, uint8_t zSize)
{
sizeof_float = sizeof(float);
sizeof_double = sizeof(double);
kf->xhatSize = xhatSize;
kf->uSize = uSize;
kf->zSize = zSize;
kf->MeasurementValidNum = 0;
// measurement flags
kf->MeasurementMap = (uint8_t *)user_malloc(sizeof(uint8_t) * zSize);
memset(kf->MeasurementMap, 0, sizeof(uint8_t) * zSize);
kf->MeasurementDegree = (float *)user_malloc(sizeof_float * zSize);
memset(kf->MeasurementDegree, 0, sizeof_float * zSize);
kf->MatR_DiagonalElements = (float *)user_malloc(sizeof_float * zSize);
memset(kf->MatR_DiagonalElements, 0, sizeof_float * zSize);
kf->StateMinVariance = (float *)user_malloc(sizeof_float * xhatSize);
memset(kf->StateMinVariance, 0, sizeof_float * xhatSize);
kf->temp = (uint8_t *)user_malloc(sizeof(uint8_t) * zSize);
memset(kf->temp, 0, sizeof(uint8_t) * zSize);
// filter data
kf->FilteredValue = (float *)user_malloc(sizeof_float * xhatSize);
memset(kf->FilteredValue, 0, sizeof_float * xhatSize);
kf->MeasuredVector = (float *)user_malloc(sizeof_float * zSize);
memset(kf->MeasuredVector, 0, sizeof_float * zSize);
kf->ControlVector = (float *)user_malloc(sizeof_float * uSize);
memset(kf->ControlVector, 0, sizeof_float * uSize);
// xhat x(k|k)
kf->xhat_data = (float *)user_malloc(sizeof_float * xhatSize);
memset(kf->xhat_data, 0, sizeof_float * xhatSize);
Matrix_Init(&kf->xhat, kf->xhatSize, 1, (float *)kf->xhat_data);
// xhatminus x(k|k-1)
kf->xhatminus_data = (float *)user_malloc(sizeof_float * xhatSize);
memset(kf->xhatminus_data, 0, sizeof_float * xhatSize);
Matrix_Init(&kf->xhatminus, kf->xhatSize, 1, (float *)kf->xhatminus_data);
if (uSize != 0)
{
// control vector u
kf->u_data = (float *)user_malloc(sizeof_float * uSize);
memset(kf->u_data, 0, sizeof_float * uSize);
Matrix_Init(&kf->u, kf->uSize, 1, (float *)kf->u_data);
}
// measurement vector z
kf->z_data = (float *)user_malloc(sizeof_float * zSize);
memset(kf->z_data, 0, sizeof_float * zSize);
Matrix_Init(&kf->z, kf->zSize, 1, (float *)kf->z_data);
// covariance matrix P(k|k)
kf->P_data = (float *)user_malloc(sizeof_float * xhatSize * xhatSize);
memset(kf->P_data, 0, sizeof_float * xhatSize * xhatSize);
Matrix_Init(&kf->P, kf->xhatSize, kf->xhatSize, (float *)kf->P_data);
// create covariance matrix P(k|k-1)
kf->Pminus_data = (float *)user_malloc(sizeof_float * xhatSize * xhatSize);
memset(kf->Pminus_data, 0, sizeof_float * xhatSize * xhatSize);
Matrix_Init(&kf->Pminus, kf->xhatSize, kf->xhatSize, (float *)kf->Pminus_data);
// state transition matrix F FT
kf->F_data = (float *)user_malloc(sizeof_float * xhatSize * xhatSize);
kf->FT_data = (float *)user_malloc(sizeof_float * xhatSize * xhatSize);
memset(kf->F_data, 0, sizeof_float * xhatSize * xhatSize);
memset(kf->FT_data, 0, sizeof_float * xhatSize * xhatSize);
Matrix_Init(&kf->F, kf->xhatSize, kf->xhatSize, (float *)kf->F_data);
Matrix_Init(&kf->FT, kf->xhatSize, kf->xhatSize, (float *)kf->FT_data);
if (uSize != 0)
{
// control matrix B
kf->B_data = (float *)user_malloc(sizeof_float * xhatSize * uSize);
memset(kf->B_data, 0, sizeof_float * xhatSize * uSize);
Matrix_Init(&kf->B, kf->xhatSize, kf->uSize, (float *)kf->B_data);
}
// measurement matrix H
kf->H_data = (float *)user_malloc(sizeof_float * zSize * xhatSize);
kf->HT_data = (float *)user_malloc(sizeof_float * xhatSize * zSize);
memset(kf->H_data, 0, sizeof_float * zSize * xhatSize);
memset(kf->HT_data, 0, sizeof_float * xhatSize * zSize);
Matrix_Init(&kf->H, kf->zSize, kf->xhatSize, (float *)kf->H_data);
Matrix_Init(&kf->HT, kf->xhatSize, kf->zSize, (float *)kf->HT_data);
// process noise covariance matrix Q
kf->Q_data = (float *)user_malloc(sizeof_float * xhatSize * xhatSize);
memset(kf->Q_data, 0, sizeof_float * xhatSize * xhatSize);
Matrix_Init(&kf->Q, kf->xhatSize, kf->xhatSize, (float *)kf->Q_data);
// measurement noise covariance matrix R
kf->R_data = (float *)user_malloc(sizeof_float * zSize * zSize);
memset(kf->R_data, 0, sizeof_float * zSize * zSize);
Matrix_Init(&kf->R, kf->zSize, kf->zSize, (float *)kf->R_data);
// kalman gain K
kf->K_data = (float *)user_malloc(sizeof_float * xhatSize * zSize);
memset(kf->K_data, 0, sizeof_float * xhatSize * zSize);
Matrix_Init(&kf->K, kf->xhatSize, kf->zSize, (float *)kf->K_data);
kf->S_data = (float *)user_malloc(sizeof_float * kf->xhatSize * kf->xhatSize);
kf->temp_matrix_data = (float *)user_malloc(sizeof_float * kf->xhatSize * kf->xhatSize);
kf->temp_matrix_data1 = (float *)user_malloc(sizeof_float * kf->xhatSize * kf->xhatSize);
kf->temp_vector_data = (float *)user_malloc(sizeof_float * kf->xhatSize);
kf->temp_vector_data1 = (float *)user_malloc(sizeof_float * kf->xhatSize);
Matrix_Init(&kf->S, kf->xhatSize, kf->xhatSize, (float *)kf->S_data);
Matrix_Init(&kf->temp_matrix, kf->xhatSize, kf->xhatSize, (float *)kf->temp_matrix_data);
Matrix_Init(&kf->temp_matrix1, kf->xhatSize, kf->xhatSize, (float *)kf->temp_matrix_data1);
Matrix_Init(&kf->temp_vector, kf->xhatSize, 1, (float *)kf->temp_vector_data);
Matrix_Init(&kf->temp_vector1, kf->xhatSize, 1, (float *)kf->temp_vector_data1);
kf->SkipEq1 = 0;
kf->SkipEq2 = 0;
kf->SkipEq3 = 0;
kf->SkipEq4 = 0;
kf->SkipEq5 = 0;
}
void Kalman_Filter_Measure(KalmanFilter_t *kf)
{
// 矩阵H K R根据量测情况自动调整
// matrix H K R auto adjustment
if (kf->UseAutoAdjustment != 0)
H_K_R_Adjustment(kf);
else
{
memcpy(kf->z_data, kf->MeasuredVector, sizeof_float * kf->zSize);
memset(kf->MeasuredVector, 0, sizeof_float * kf->zSize);
}
memcpy(kf->u_data, kf->ControlVector, sizeof_float * kf->uSize);
}
void Kalman_Filter_xhatMinusUpdate(KalmanFilter_t *kf)
{
if (!kf->SkipEq1)
{
if (kf->uSize > 0)
{
kf->temp_vector.numRows = kf->xhatSize;
kf->temp_vector.numCols = 1;
kf->MatStatus = Matrix_Multiply(&kf->F, &kf->xhat, &kf->temp_vector);
kf->temp_vector1.numRows = kf->xhatSize;
kf->temp_vector1.numCols = 1;
kf->MatStatus = Matrix_Multiply(&kf->B, &kf->u, &kf->temp_vector1);
kf->MatStatus = Matrix_Add(&kf->temp_vector, &kf->temp_vector1, &kf->xhatminus);
}
else
{
kf->MatStatus = Matrix_Multiply(&kf->F, &kf->xhat, &kf->xhatminus);
}
}
}
void Kalman_Filter_PminusUpdate(KalmanFilter_t *kf)
{
if (!kf->SkipEq2)
{
kf->MatStatus = Matrix_Transpose(&kf->F, &kf->FT);
kf->MatStatus = Matrix_Multiply(&kf->F, &kf->P, &kf->Pminus);
kf->temp_matrix.numRows = kf->Pminus.numRows;
kf->temp_matrix.numCols = kf->FT.numCols;
kf->MatStatus = Matrix_Multiply(&kf->Pminus, &kf->FT, &kf->temp_matrix); // temp_matrix = F P(k-1) FT
kf->MatStatus = Matrix_Add(&kf->temp_matrix, &kf->Q, &kf->Pminus);
}
}
void Kalman_Filter_SetK(KalmanFilter_t *kf)
{
if (!kf->SkipEq3)
{
kf->MatStatus = Matrix_Transpose(&kf->H, &kf->HT); // z|x => x|z
kf->temp_matrix.numRows = kf->H.numRows;
kf->temp_matrix.numCols = kf->Pminus.numCols;
kf->MatStatus = Matrix_Multiply(&kf->H, &kf->Pminus, &kf->temp_matrix); // temp_matrix = H·P'(k)
kf->temp_matrix1.numRows = kf->temp_matrix.numRows;
kf->temp_matrix1.numCols = kf->HT.numCols;
kf->MatStatus = Matrix_Multiply(&kf->temp_matrix, &kf->HT, &kf->temp_matrix1); // temp_matrix1 = H·P'(k)·HT
kf->S.numRows = kf->R.numRows;
kf->S.numCols = kf->R.numCols;
kf->MatStatus = Matrix_Add(&kf->temp_matrix1, &kf->R, &kf->S); // S = H P'(k) HT + R
kf->MatStatus = Matrix_Inverse(&kf->S, &kf->temp_matrix1); // temp_matrix1 = inv(H·P'(k)·HT + R)
kf->temp_matrix.numRows = kf->Pminus.numRows;
kf->temp_matrix.numCols = kf->HT.numCols;
kf->MatStatus = Matrix_Multiply(&kf->Pminus, &kf->HT, &kf->temp_matrix); // temp_matrix = P'(k)·HT
kf->MatStatus = Matrix_Multiply(&kf->temp_matrix, &kf->temp_matrix1, &kf->K);
}
}
void Kalman_Filter_xhatUpdate(KalmanFilter_t *kf)
{
if (!kf->SkipEq4)
{
kf->temp_vector.numRows = kf->H.numRows;
kf->temp_vector.numCols = 1;
kf->MatStatus = Matrix_Multiply(&kf->H, &kf->xhatminus, &kf->temp_vector); // temp_vector = H xhat'(k)
kf->temp_vector1.numRows = kf->z.numRows;
kf->temp_vector1.numCols = 1;
kf->MatStatus = Matrix_Subtract(&kf->z, &kf->temp_vector, &kf->temp_vector1); // temp_vector1 = z(k) - H·xhat'(k)
kf->temp_vector.numRows = kf->K.numRows;
kf->temp_vector.numCols = 1;
kf->MatStatus = Matrix_Multiply(&kf->K, &kf->temp_vector1, &kf->temp_vector); // temp_vector = K(k)·(z(k) - H·xhat'(k))
kf->MatStatus = Matrix_Add(&kf->xhatminus, &kf->temp_vector, &kf->xhat);
}
}
void Kalman_Filter_P_Update(KalmanFilter_t *kf)
{
if (!kf->SkipEq5)
{
kf->temp_matrix.numRows = kf->K.numRows;
kf->temp_matrix.numCols = kf->H.numCols;
kf->temp_matrix1.numRows = kf->temp_matrix.numRows;
kf->temp_matrix1.numCols = kf->Pminus.numCols;
kf->MatStatus = Matrix_Multiply(&kf->K, &kf->H, &kf->temp_matrix); // temp_matrix = K(k)·H
kf->MatStatus = Matrix_Multiply(&kf->temp_matrix, &kf->Pminus, &kf->temp_matrix1); // temp_matrix1 = K(k)·H·P'(k)
kf->MatStatus = Matrix_Subtract(&kf->Pminus, &kf->temp_matrix1, &kf->P);
}
}
/**
* @brief 执行卡尔曼滤波黄金五式,提供了用户定义函数,可以替代五个中的任意一个环节,方便自行扩展为EKF/UKF/ESKF/AUKF等
*
* @param kf kf类型定义
* @return float* 返回滤波值
*/
float *Kalman_Filter_Update(KalmanFilter_t *kf)
{
// 0. 获取量测信息
Kalman_Filter_Measure(kf);
if (kf->User_Func0_f != NULL)
kf->User_Func0_f(kf);
// 先验估计
// 1. xhat'(k)= A·xhat(k-1) + B·u
Kalman_Filter_xhatMinusUpdate(kf);
if (kf->User_Func1_f != NULL)
kf->User_Func1_f(kf);
// 预测更新
// 2. P'(k) = A·P(k-1)·AT + Q
Kalman_Filter_PminusUpdate(kf);
if (kf->User_Func2_f != NULL)
kf->User_Func2_f(kf);
if (kf->MeasurementValidNum != 0 || kf->UseAutoAdjustment == 0)
{
// 量测更新
// 3. K(k) = P'(k)·HT / (H·P'(k)·HT + R)
Kalman_Filter_SetK(kf);
if (kf->User_Func3_f != NULL)
kf->User_Func3_f(kf);
// 融合
// 4. xhat(k) = xhat'(k) + K(k)·(z(k) - H·xhat'(k))
Kalman_Filter_xhatUpdate(kf);
if (kf->User_Func4_f != NULL)
kf->User_Func4_f(kf);
// 修正方差
// 5. P(k) = (1-K(k)·H)·P'(k) ==> P(k) = P'(k)-K(k)·H·P'(k)
Kalman_Filter_P_Update(kf);
}
else
{
// 无有效量测,仅预测
// xhat(k) = xhat'(k)
// P(k) = P'(k)
memcpy(kf->xhat_data, kf->xhatminus_data, sizeof_float * kf->xhatSize);
memcpy(kf->P_data, kf->Pminus_data, sizeof_float * kf->xhatSize * kf->xhatSize);
}
// 自定义函数,可以提供后处理等
if (kf->User_Func5_f != NULL)
kf->User_Func5_f(kf);
// 避免滤波器过度收敛
// suppress filter excessive convergence
for (uint8_t i = 0; i < kf->xhatSize; ++i)
{
if (kf->P_data[i * kf->xhatSize + i] < kf->StateMinVariance[i])
kf->P_data[i * kf->xhatSize + i] = kf->StateMinVariance[i];
}
memcpy(kf->FilteredValue, kf->xhat_data, sizeof_float * kf->xhatSize);
if (kf->User_Func6_f != NULL)
kf->User_Func6_f(kf);
return kf->FilteredValue;
}
static void H_K_R_Adjustment(KalmanFilter_t *kf)
{
kf->MeasurementValidNum = 0;
memcpy(kf->z_data, kf->MeasuredVector, sizeof_float * kf->zSize);
memset(kf->MeasuredVector, 0, sizeof_float * kf->zSize);
// 识别量测数据有效性并调整矩阵H R K
// recognize measurement validity and adjust matrices H R K
memset(kf->R_data, 0, sizeof_float * kf->zSize * kf->zSize);
memset(kf->H_data, 0, sizeof_float * kf->xhatSize * kf->zSize);
for (uint8_t i = 0; i < kf->zSize; ++i)
{
if (kf->z_data[i] != 0)
{
// 重构向量z
// rebuild vector z
kf->z_data[kf->MeasurementValidNum] = kf->z_data[i];
kf->temp[kf->MeasurementValidNum] = i;
// 重构矩阵H
// rebuild matrix H
kf->H_data[kf->xhatSize * kf->MeasurementValidNum + kf->MeasurementMap[i] - 1] = kf->MeasurementDegree[i];
kf->MeasurementValidNum++;
}
}
for (uint8_t i = 0; i < kf->MeasurementValidNum; ++i)
{
// 重构矩阵R
// rebuild matrix R
kf->R_data[i * kf->MeasurementValidNum + i] = kf->MatR_DiagonalElements[kf->temp[i]];
}
// 调整矩阵维数
// adjust the dimensions of system matrices
kf->H.numRows = kf->MeasurementValidNum;
kf->H.numCols = kf->xhatSize;
kf->HT.numRows = kf->xhatSize;
kf->HT.numCols = kf->MeasurementValidNum;
kf->R.numRows = kf->MeasurementValidNum;
kf->R.numCols = kf->MeasurementValidNum;
kf->K.numRows = kf->xhatSize;
kf->K.numCols = kf->MeasurementValidNum;
kf->z.numRows = kf->MeasurementValidNum;
}
modules/algorithm/kalman_filter.h
+122
View File
@@ -0,0 +1,122 @@
/**
******************************************************************************
* @file kalman filter.h
* @author Wang Hongxi
* @version V1.2.2
* @date 2022/1/8
* @brief
******************************************************************************
* @attention
*
******************************************************************************
*/
#ifndef __KALMAN_FILTER_H
#define __KALMAN_FILTER_H
// cortex-m4 DSP lib
/*
#define __CC_ARM // Keil
#define ARM_MATH_CM4
#define ARM_MATH_MATRIX_CHECK
#define ARM_MATH_ROUNDING
#define ARM_MATH_DSP // define in arm_math.h
*/
//#include "stm32f407xx.h"
#include "arm_math.h"
//#include "dsp/matrix_functions.h"
#include "math.h"
#include "stdint.h"
#include "stdlib.h"
#ifndef user_malloc
#ifdef _CMSIS_OS_H
#define user_malloc pvPortMalloc
#else
#define user_malloc malloc
#endif
#endif
// 若运算速度不够,可以使用q31代替f32,但是精度会降低
#define mat arm_matrix_instance_f32
#define Matrix_Init arm_mat_init_f32
#define Matrix_Add arm_mat_add_f32
#define Matrix_Subtract arm_mat_sub_f32
#define Matrix_Multiply arm_mat_mult_f32
#define Matrix_Transpose arm_mat_trans_f32
#define Matrix_Inverse arm_mat_inverse_f32
typedef struct kf_t
{
float *FilteredValue;
float *MeasuredVector;
float *ControlVector;
uint8_t xhatSize;
uint8_t uSize;
uint8_t zSize;
uint8_t UseAutoAdjustment;
uint8_t MeasurementValidNum;
uint8_t *MeasurementMap; // 量测与状态的关系 how measurement relates to the state
float *MeasurementDegree; // 测量值对应H矩阵元素值 elements of each measurement in H
float *MatR_DiagonalElements; // 量测方差 variance for each measurement
float *StateMinVariance; // 最小方差 避免方差过度收敛 suppress filter excessive convergence
uint8_t *temp;
// 配合用户定义函数使用,作为标志位用于判断是否要跳过标准KF中五个环节中的任意一个
uint8_t SkipEq1, SkipEq2, SkipEq3, SkipEq4, SkipEq5;
// definiion of struct mat: rows & cols & pointer to vars
mat xhat; // x(k|k)
mat xhatminus; // x(k|k-1)
mat u; // control vector u
mat z; // measurement vector z
mat P; // covariance matrix P(k|k)
mat Pminus; // covariance matrix P(k|k-1)
mat F, FT; // state transition matrix F FT
mat B; // control matrix B
mat H, HT; // measurement matrix H
mat Q; // process noise covariance matrix Q
mat R; // measurement noise covariance matrix R
mat K; // kalman gain K
mat S, temp_matrix, temp_matrix1, temp_vector, temp_vector1;
int8_t MatStatus;
// 用户定义函数,可以替换或扩展基准KF的功能
void (*User_Func0_f)(struct kf_t *kf);
void (*User_Func1_f)(struct kf_t *kf);
void (*User_Func2_f)(struct kf_t *kf);
void (*User_Func3_f)(struct kf_t *kf);
void (*User_Func4_f)(struct kf_t *kf);
void (*User_Func5_f)(struct kf_t *kf);
void (*User_Func6_f)(struct kf_t *kf);
// 矩阵存储空间指针
float *xhat_data, *xhatminus_data;
float *u_data;
float *z_data;
float *P_data, *Pminus_data;
float *F_data, *FT_data;
float *B_data;
float *H_data, *HT_data;
float *Q_data;
float *R_data;
float *K_data;
float *S_data, *temp_matrix_data, *temp_matrix_data1, *temp_vector_data, *temp_vector_data1;
} KalmanFilter_t;
extern uint16_t sizeof_float, sizeof_double;
void Kalman_Filter_Init(KalmanFilter_t *kf, uint8_t xhatSize, uint8_t uSize, uint8_t zSize);
void Kalman_Filter_Measure(KalmanFilter_t *kf);
void Kalman_Filter_xhatMinusUpdate(KalmanFilter_t *kf);
void Kalman_Filter_PminusUpdate(KalmanFilter_t *kf);
void Kalman_Filter_SetK(KalmanFilter_t *kf);
void Kalman_Filter_xhatUpdate(KalmanFilter_t *kf);
void Kalman_Filter_P_Update(KalmanFilter_t *kf);
float *Kalman_Filter_Update(KalmanFilter_t *kf);
#endif //__KALMAN_FILTER_H
modules/algorithm/user_lib.c
+214
View File
@@ -0,0 +1,214 @@
/**
******************************************************************************
* @file user_lib.c
* @author Wang Hongxi
* @author modified by neozng
* @version 0.2 beta
* @date 2021/2/18
* @brief
******************************************************************************
* @attention
*
******************************************************************************
*/
#include "stdlib.h"
#include "memory.h"
#include "user_lib.h"
#include "math.h"
#include "main.h"
#ifdef _CMSIS_OS_H
#define user_malloc pvPortMalloc
#else
#define user_malloc malloc
#endif
void *zmalloc(size_t size)
{
void *ptr = malloc(size);
memset(ptr, 0, size);
return ptr;
}
// 快速开方
float Sqrt(float x)
{
float y;
float delta;
float maxError;
if (x <= 0)
{
return 0;
}
// initial guess
y = x / 2;
// refine
maxError = x * 0.001f;
do
{
delta = (y * y) - x;
y -= delta / (2 * y);
} while (delta > maxError || delta < -maxError);
return y;
}
// 绝对值限制
float abs_limit(float num, float Limit)
{
if (num > Limit)
{
num = Limit;
}
else if (num < -Limit)
{
num = -Limit;
}
return num;
}
// 判断符号位
float sign(float value)
{
if (value >= 0.0f)
{
return 1.0f;
}
else
{
return -1.0f;
}
}
// 浮点死区
float float_deadband(float Value, float minValue, float maxValue)
{
if (Value < maxValue && Value > minValue)
{
Value = 0.0f;
}
return Value;
}
// 限幅函数
float float_constrain(float Value, float minValue, float maxValue)
{
if (Value < minValue)
return minValue;
else if (Value > maxValue)
return maxValue;
else
return Value;
}
// 限幅函数
int16_t int16_constrain(int16_t Value, int16_t minValue, int16_t maxValue)
{
if (Value < minValue)
return minValue;
else if (Value > maxValue)
return maxValue;
else
return Value;
}
// 循环限幅函数
float loop_float_constrain(float Input, float minValue, float maxValue)
{
if (maxValue < minValue)
{
return Input;
}
if (Input > maxValue)
{
float len = maxValue - minValue;
while (Input > maxValue)
{
Input -= len;
}
}
else if (Input < minValue)
{
float len = maxValue - minValue;
while (Input < minValue)
{
Input += len;
}
}
return Input;
}
// 弧度格式化为-PI~PI
// 角度格式化为-180~180
float theta_format(float Ang)
{
return loop_float_constrain(Ang, -180.0f, 180.0f);
}
int float_rounding(float raw)
{
static int integer;
static float decimal;
integer = (int)raw;
decimal = raw - integer;
if (decimal > 0.5f)
integer++;
return integer;
}
// 三维向量归一化
float *Norm3d(float *v)
{
float len = Sqrt(v[0] * v[0] + v[1] * v[1] + v[2] * v[2]);
v[0] /= len;
v[1] /= len;
v[2] /= len;
return v;
}
// 计算模长
float NormOf3d(float *v)
{
return Sqrt(v[0] * v[0] + v[1] * v[1] + v[2] * v[2]);
}
// 三维向量叉乘v1 x v2
void Cross3d(float *v1, float *v2, float *res)
{
res[0] = v1[1] * v2[2] - v1[2] * v2[1];
res[1] = v1[2] * v2[0] - v1[0] * v2[2];
res[2] = v1[0] * v2[1] - v1[1] * v2[0];
}
// 三维向量点乘
float Dot3d(float *v1, float *v2)
{
return v1[0] * v2[0] + v1[1] * v2[1] + v1[2] * v2[2];
}
// 均值滤波,删除buffer中的最后一个元素,填入新的元素并求平均值
float AverageFilter(float new_data, float *buf, uint8_t len)
{
float sum = 0;
for (uint8_t i = 0; i < len - 1; i++)
{
buf[i] = buf[i + 1];
sum += buf[i];
}
buf[len - 1] = new_data;
sum += new_data;
return sum / len;
}
void MatInit(mat *m, uint8_t row, uint8_t col)
{
m->numCols = col;
m->numRows = row;
m->pData = (float *)zmalloc(row * col * sizeof(float));
}
modules/algorithm/user_lib.h
+126
View File
@@ -0,0 +1,126 @@
/**
******************************************************************************
* @file user_lib.h
* @author Wang Hongxi
* @version V1.0.0
* @date 2021/2/18
* @brief
******************************************************************************
* @attention
*
******************************************************************************
*/
#ifndef _USER_LIB_H
#define _USER_LIB_H
#include "stdint.h"
#include "main.h"
#include "cmsis_os.h"
//#include "stm32f407xx.h"
#include "arm_math.h"
#ifndef user_malloc
#ifdef _CMSIS_OS_H
#define user_malloc pvPortMalloc
#else
#define user_malloc malloc
#endif
#endif
#define msin(x) (arm_sin_f32(x))
#define mcos(x) (arm_cos_f32(x))
typedef arm_matrix_instance_f32 mat;
// 若运算速度不够,可以使用q31代替f32,但是精度会降低
#define MatAdd arm_mat_add_f32
#define MatSubtract arm_mat_sub_f32
#define MatMultiply arm_mat_mult_f32
#define MatTranspose arm_mat_trans_f32
#define MatInverse arm_mat_inverse_f32
void MatInit(mat *m, uint8_t row, uint8_t col);
/* boolean type definitions */
#ifndef TRUE
#define TRUE 1 /**< boolean true */
#endif
#ifndef FALSE
#define FALSE 0 /**< boolean fails */
#endif
/* circumference ratio */
#ifndef PI
#define PI 3.14159265354f
#endif
#define VAL_LIMIT(val, min, max) \
do \
{ \
if ((val) <= (min)) \
{ \
(val) = (min); \
} \
else if ((val) >= (max)) \
{ \
(val) = (max); \
} \
} while (0)
#define ANGLE_LIMIT_360(val, angle) \
do \
{ \
(val) = (angle) - (int)(angle); \
(val) += (int)(angle) % 360; \
} while (0)
#define ANGLE_LIMIT_360_TO_180(val) \
do \
{ \
if ((val) > 180) \
(val) -= 360; \
} while (0)
#define VAL_MIN(a, b) ((a) < (b) ? (a) : (b))
#define VAL_MAX(a, b) ((a) > (b) ? (a) : (b))
/**
* @brief 返回一块干净的内�?,不过仍然需要强制转�?为你需要的类型
*
* @param size 分配大小
* @return void*
*/
void *zmalloc(size_t size);
// ���ٿ���
float Sqrt(float x);
// ��������
float abs_limit(float num, float Limit);
// �жϷ���λ
float sign(float value);
// ��������
float float_deadband(float Value, float minValue, float maxValue);
// �޷�����
float float_constrain(float Value, float minValue, float maxValue);
// �޷�����
int16_t int16_constrain(int16_t Value, int16_t minValue, int16_t maxValue);
// ѭ���޷�����
float loop_float_constrain(float Input, float minValue, float maxValue);
// �Ƕ� ���޷� 180 ~ -180
float theta_format(float Ang);
int float_rounding(float raw);
float *Norm3d(float *v);
float NormOf3d(float *v);
void Cross3d(float *v1, float *v2, float *res);
float Dot3d(float *v1, float *v2);
float AverageFilter(float new_data, float *buf, uint8_t len);
#define rad_format(Ang) loop_float_constrain((Ang), -PI, PI)
#endif