From be86c08ef4261a0705c6f3dab6e74d013d654499 Mon Sep 17 00:00:00 2001 From: Robofish <1683502971@qq.com> Date: Mon, 2 Feb 2026 02:46:09 +0800 Subject: [PATCH] =?UTF-8?q?=E6=B7=BB=E5=8A=A0=E5=8D=A1=E5=B0=94=E6=9B=BC?= =?UTF-8?q?=E6=BB=A4=E6=B3=A2=E5=99=A8?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- User/component/kalman_filter.c | 991 ++++++++++++++++++--------------- User/component/kalman_filter.h | 267 ++++++--- 2 files changed, 724 insertions(+), 534 deletions(-) diff --git a/User/component/kalman_filter.c b/User/component/kalman_filter.c index bf3bbaa..9ff4994 100644 --- a/User/component/kalman_filter.c +++ b/User/component/kalman_filter.c @@ -1,488 +1,591 @@ -/** - ****************************************************************************** - * @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; - * } - ****************************************************************************** - */ +/* + 卡尔曼滤波器 modified from wang hongxi + 支持动态量测调整，使用ARM CMSIS DSP优化矩阵运算 + + 主要特性： + - 基于量测有效性的 H、R、K 矩阵动态调整 + - 支持不同传感器采样频率 + - 矩阵 P 防过度收敛机制 + - ARM CMSIS DSP 优化的矩阵运算 + - 可扩展架构，支持用户自定义函数（EKF/UKF/ESKF） + + 使用说明： + 1. 矩阵初始化：P、F、Q 使用标准初始化方式 + H、R 在使用自动调整时需要配置量测映射 + + 2. 自动调整模式 (use_auto_adjustment = 1)： + - 提供 measurement_map：每个量测对应的状态索引 + - 提供 measurement_degree：H 矩阵元素值 + - 提供 mat_r_diagonal_elements：量测噪声方差 + + 3. 固定模式 (use_auto_adjustment = 0)： + - 像初始化 P 矩阵那样正常初始化 z、H、R + + 4. 量测更新： + - 在传感器回调函数中更新 measured_vector + - 值为 0 表示量测无效 + - 向量在每次 KF 更新后会被重置为 0 + + 5. 防过度收敛： + - 设置 state_min_variance 防止 P 矩阵过度收敛 + - 帮助滤波器适应缓慢变化的状态 + + 使用示例：高度估计 + 状态向量 x = + | 高度 | + | 速度 | + | 加速度 | + + KF_t Height_KF; + + void INS_Task_Init(void) + { + // 初始协方差矩阵 P + static float P_Init[9] = + { + 10, 0, 0, + 0, 30, 0, + 0, 0, 10, + }; + + // 状态转移矩阵 F（根据运动学模型） + static float F_Init[9] = + { + 1, dt, 0.5*dt*dt, + 0, 1, dt, + 0, 0, 1, + }; + + // 过程噪声协方差矩阵 Q + 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.use_auto_adjustment = 1; + + // 量测映射：[气压高度对应状态1, GPS高度对应状态1, 加速度计对应状态3] + static uint8_t measurement_reference[3] = {1, 1, 3}; + + // 量测系数（H矩阵元素值） + static float measurement_degree[3] = {1, 1, 1}; + // 根据 measurement_reference 与 measurement_degree 生成 H 矩阵如下 + // （在当前周期全部量测数据有效的情况下） + // |1 0 0| + // |1 0 0| + // |0 0 1| + + // 量测噪声方差（R矩阵对角元素） + static float mat_r_diagonal_elements[3] = {30, 25, 35}; + // 根据 mat_r_diagonal_elements 生成 R 矩阵如下 + // （在当前周期全部量测数据有效的情况下） + // |30 0 0| + // | 0 25 0| + // | 0 0 35| + + // 初始化卡尔曼滤波器（状态维数3，控制维数0，量测维数3） + KF_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.measurement_map, measurement_reference, + sizeof(measurement_reference)); + memcpy(Height_KF.measurement_degree, measurement_degree, + sizeof(measurement_degree)); + memcpy(Height_KF.mat_r_diagonal_elements, mat_r_diagonal_elements, + sizeof(mat_r_diagonal_elements)); + memcpy(Height_KF.state_min_variance, state_min_variance, + sizeof(state_min_variance)); + } + + void INS_Task(void const *pvParameters) + { + // 循环更新卡尔曼滤波器 + KF_Update(&Height_KF); + vTaskDelay(ts); + } + + // 传感器回调函数示例：在数据就绪时更新 measured_vector + void Barometer_Read_Over(void) + { + ...... + INS_KF.measured_vector[0] = baro_height; // 气压计高度 + } + + void GPS_Read_Over(void) + { + ...... + INS_KF.measured_vector[1] = GPS_height; // GPS高度 + } + + void Acc_Data_Process(void) + { + ...... + INS_KF.measured_vector[2] = acc.z; // Z轴加速度 + } +*/ #include "kalman_filter.h" -uint16_t sizeof_float, sizeof_double; +/* USER INCLUDE BEGIN */ -static void H_K_R_Adjustment(KalmanFilter_t *kf); +/* USER INCLUDE END */ + +/* USER DEFINE BEGIN */ + +/* USER DEFINE END */ + +/* 私有函数声明 */ +static void KF_AdjustHKR(KF_t *kf); /** - * @brief 初始化矩阵维度信息并为矩阵分配空间 + * @brief 初始化卡尔曼滤波器并分配矩阵内存 * - * @param kf kf类型定义 - * @param xhatSize 状态变量维度 - * @param uSize 控制变量维度 - * @param zSize 观测量维度 + * @param kf 卡尔曼滤波器结构体指针 + * @param xhat_size 状态向量维度 + * @param u_size 控制向量维度（无控制输入时设为0） + * @param z_size 量测向量维度 + * @return int8_t 0对应没有错误 */ -void Kalman_Filter_Init(KalmanFilter_t *kf, uint8_t xhatSize, uint8_t uSize, uint8_t zSize) -{ - sizeof_float = sizeof(float); - sizeof_double = sizeof(double); +int8_t KF_Init(KF_t *kf, uint8_t xhat_size, uint8_t u_size, uint8_t z_size) { + if (kf == NULL) return -1; - kf->xhatSize = xhatSize; - kf->uSize = uSize; - kf->zSize = zSize; + kf->xhat_size = xhat_size; + kf->u_size = u_size; + kf->z_size = z_size; - kf->MeasurementValidNum = 0; + kf->measurement_valid_num = 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); + /* 量测标志分配 */ + kf->measurement_map = (uint8_t *)user_malloc(sizeof(uint8_t) * z_size); + memset(kf->measurement_map, 0, sizeof(uint8_t) * z_size); - // 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); + kf->measurement_degree = (float *)user_malloc(sizeof(float) * z_size); + memset(kf->measurement_degree, 0, sizeof(float) * z_size); - // 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); + kf->mat_r_diagonal_elements = (float *)user_malloc(sizeof(float) * z_size); + memset(kf->mat_r_diagonal_elements, 0, sizeof(float) * z_size); - // 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); + kf->state_min_variance = (float *)user_malloc(sizeof(float) * xhat_size); + memset(kf->state_min_variance, 0, sizeof(float) * xhat_size); - 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); - } + kf->temp = (uint8_t *)user_malloc(sizeof(uint8_t) * z_size); + memset(kf->temp, 0, sizeof(uint8_t) * z_size); - // 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); + /* 滤波数据分配 */ + kf->filtered_value = (float *)user_malloc(sizeof(float) * xhat_size); + memset(kf->filtered_value, 0, sizeof(float) * xhat_size); - // 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); + kf->measured_vector = (float *)user_malloc(sizeof(float) * z_size); + memset(kf->measured_vector, 0, sizeof(float) * z_size); - // 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); + kf->control_vector = (float *)user_malloc(sizeof(float) * u_size); + memset(kf->control_vector, 0, sizeof(float) * u_size); - // 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); + /* 状态估计 xhat x(k|k) */ + kf->xhat_data = (float *)user_malloc(sizeof(float) * xhat_size); + memset(kf->xhat_data, 0, sizeof(float) * xhat_size); + KF_MatInit(&kf->xhat, kf->xhat_size, 1, kf->xhat_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); - } + /* 先验状态估计 xhatminus x(k|k-1) */ + kf->xhatminus_data = (float *)user_malloc(sizeof(float) * xhat_size); + memset(kf->xhatminus_data, 0, sizeof(float) * xhat_size); + KF_MatInit(&kf->xhatminus, kf->xhat_size, 1, kf->xhatminus_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); + /* 控制向量 u */ + if (u_size != 0) { + kf->u_data = (float *)user_malloc(sizeof(float) * u_size); + memset(kf->u_data, 0, sizeof(float) * u_size); + KF_MatInit(&kf->u, kf->u_size, 1, kf->u_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); + /* 量测向量 z */ + kf->z_data = (float *)user_malloc(sizeof(float) * z_size); + memset(kf->z_data, 0, sizeof(float) * z_size); + KF_MatInit(&kf->z, kf->z_size, 1, kf->z_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); + /* 协方差矩阵 P(k|k) */ + kf->P_data = (float *)user_malloc(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); - // 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); + /* 先验协方差矩阵 P(k|k-1) */ + kf->Pminus_data = (float *)user_malloc(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->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); + /* 状态转移矩阵 F 及其转置 FT */ + kf->F_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->FT_data, 0, sizeof(float) * xhat_size * xhat_size); + 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->SkipEq1 = 0; - kf->SkipEq2 = 0; - kf->SkipEq3 = 0; - kf->SkipEq4 = 0; - kf->SkipEq5 = 0; -} + /* 控制矩阵 B */ + if (u_size != 0) { + kf->B_data = (float *)user_malloc(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); + } -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); - } + /* 量测矩阵 H 及其转置 HT */ + kf->H_data = (float *)user_malloc(sizeof(float) * z_size * xhat_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->HT_data, 0, sizeof(float) * xhat_size * z_size); + 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); - memcpy(kf->u_data, kf->ControlVector, sizeof_float * kf->uSize); -} + /* 过程噪声协方差矩阵 Q */ + kf->Q_data = (float *)user_malloc(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); -extern int stop_time; -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; -// if(stop_time==0) -// { -// kf->MatStatus = Matrix_Multiply(&kf->temp_F, &kf->xhat, &kf->temp_vector); -// } -// else -// { - 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); - } - } -} + /* 量测噪声协方差矩阵 R */ + kf->R_data = (float *)user_malloc(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); -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); - } + /* 卡尔曼增益 K */ + kf->K_data = (float *)user_malloc(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->S_data = (float *)user_malloc(sizeof(float) * xhat_size * xhat_size); + kf->temp_matrix_data = + (float *)user_malloc(sizeof(float) * xhat_size * xhat_size); + kf->temp_matrix_data1 = + (float *)user_malloc(sizeof(float) * xhat_size * xhat_size); + kf->temp_vector_data = (float *)user_malloc(sizeof(float) * xhat_size); + kf->temp_vector_data1 = (float *)user_malloc(sizeof(float) * xhat_size); + + KF_MatInit(&kf->S, kf->xhat_size, kf->xhat_size, kf->S_data); + KF_MatInit(&kf->temp_matrix, kf->xhat_size, kf->xhat_size, + kf->temp_matrix_data); + KF_MatInit(&kf->temp_matrix1, kf->xhat_size, kf->xhat_size, + kf->temp_matrix_data1); + 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->skip_eq1 = 0; + kf->skip_eq2 = 0; + kf->skip_eq3 = 0; + kf->skip_eq4 = 0; + kf->skip_eq5 = 0; + + /* 初始化用户函数指针 */ + kf->User_Func0_f = NULL; + kf->User_Func1_f = NULL; + kf->User_Func2_f = NULL; + kf->User_Func3_f = NULL; + kf->User_Func4_f = NULL; + kf->User_Func5_f = NULL; + kf->User_Func6_f = NULL; + + return 0; } /** - * @brief 执行卡尔曼滤波黄金五式,提供了用户定义函数,可以替代五个中的任意一个环节,方便自行扩展为EKF/UKF/ESKF/AUKF等 - * - * @param kf kf类型定义 - * @return float* 返回滤波值 + * @brief 获取量测并在启用自动调整时调整矩阵 + * + * @param kf 卡尔曼滤波器结构体指针 + * @return int8_t 0对应没有错误 */ -float *Kalman_Filter_Update(KalmanFilter_t *kf) -{ - // 0. 获取量测信息 - Kalman_Filter_Measure(kf); - if (kf->User_Func0_f != NULL) - kf->User_Func0_f(kf); +int8_t KF_Measure(KF_t *kf) { + if (kf == NULL) return -1; - // 先验估计 - // 1. xhat'(k)= A·xhat(k-1) + B·u - Kalman_Filter_xhatMinusUpdate(kf); - if (kf->User_Func1_f != NULL) - kf->User_Func1_f(kf); + /* 根据量测有效性自动调整 H, K, R 矩阵 */ + if (kf->use_auto_adjustment != 0) { + KF_AdjustHKR(kf); + } else { + memcpy(kf->z_data, kf->measured_vector, sizeof(float) * kf->z_size); + memset(kf->measured_vector, 0, sizeof(float) * kf->z_size); + } - // 预测更新 - // 2. P'(k) = A·P(k-1)·AT + Q - Kalman_Filter_PminusUpdate(kf); - if (kf->User_Func2_f != NULL) - kf->User_Func2_f(kf); + memcpy(kf->u_data, kf->control_vector, sizeof(float) * kf->u_size); - 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; + return 0; } -static void H_K_R_Adjustment(KalmanFilter_t *kf) -{ - kf->MeasurementValidNum = 0; +/** + * @brief 步骤1：先验状态估计 - xhat'(k) = F·xhat(k-1) + B·u + * + * @param kf 卡尔曼滤波器结构体指针 + * @return int8_t 0对应没有错误 + */ +int8_t KF_PredictState(KF_t *kf) { + if (kf == NULL) return -1; - memcpy(kf->z_data, kf->MeasuredVector, sizeof_float * kf->zSize); - memset(kf->MeasuredVector, 0, sizeof_float * kf->zSize); + if (!kf->skip_eq1) { + if (kf->u_size > 0) { + /* 有控制输入: xhat'(k) = F·xhat(k-1) + B·u */ + kf->temp_vector.numRows = kf->xhat_size; + kf->temp_vector.numCols = 1; + kf->mat_status = KF_MatMult(&kf->F, &kf->xhat, &kf->temp_vector); - // 识别量测数据有效性并调整矩阵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]]; + kf->temp_vector1.numRows = kf->xhat_size; + kf->temp_vector1.numCols = 1; + kf->mat_status = KF_MatMult(&kf->B, &kf->u, &kf->temp_vector1); + kf->mat_status = + KF_MatAdd(&kf->temp_vector, &kf->temp_vector1, &kf->xhatminus); + } else { + /* 无控制输入: xhat'(k) = F·xhat(k-1) */ + kf->mat_status = KF_MatMult(&kf->F, &kf->xhat, &kf->xhatminus); } + } - // 调整矩阵维数 - // 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; + return 0; } + +/** + * @brief 步骤2：先验协方差 - P'(k) = F·P(k-1)·F^T + Q + * + * @param kf 卡尔曼滤波器结构体指针 + * @return int8_t 0对应没有错误 + */ +int8_t KF_PredictCovariance(KF_t *kf) { + if (kf == NULL) return -1; + + if (!kf->skip_eq2) { + kf->mat_status = KF_MatTrans(&kf->F, &kf->FT); + kf->mat_status = KF_MatMult(&kf->F, &kf->P, &kf->Pminus); + kf->temp_matrix.numRows = kf->Pminus.numRows; + kf->temp_matrix.numCols = kf->FT.numCols; + /* F·P(k-1)·F^T */ + kf->mat_status = KF_MatMult(&kf->Pminus, &kf->FT, &kf->temp_matrix); + kf->mat_status = KF_MatAdd(&kf->temp_matrix, &kf->Q, &kf->Pminus); + } + + return 0; +} + +/** + * @brief 步骤3：卡尔曼增益 - K = P'(k)·H^T / (H·P'(k)·H^T + R) + * + * @param kf 卡尔曼滤波器结构体指针 + * @return int8_t 0对应没有错误 + */ +int8_t KF_CalcGain(KF_t *kf) { + if (kf == NULL) return -1; + + if (!kf->skip_eq3) { + kf->mat_status = KF_MatTrans(&kf->H, &kf->HT); + kf->temp_matrix.numRows = kf->H.numRows; + kf->temp_matrix.numCols = kf->Pminus.numCols; + /* 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; + /* H·P'(k)·H^T */ + 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)·H^T + R */ + kf->mat_status = KF_MatAdd(&kf->temp_matrix1, &kf->R, &kf->S); + /* S^-1 */ + kf->mat_status = KF_MatInv(&kf->S, &kf->temp_matrix1); + kf->temp_matrix.numRows = kf->Pminus.numRows; + kf->temp_matrix.numCols = kf->HT.numCols; + /* P'(k)·H^T */ + kf->mat_status = KF_MatMult(&kf->Pminus, &kf->HT, &kf->temp_matrix); + /* K = P'(k)·H^T·S^-1 */ + kf->mat_status = KF_MatMult(&kf->temp_matrix, &kf->temp_matrix1, &kf->K); + } + + return 0; +} + +/** + * @brief 步骤4：状态更新 - xhat(k) = xhat'(k) + K·(z - H·xhat'(k)) + * + * @param kf 卡尔曼滤波器结构体指针 + * @return int8_t 0对应没有错误 + */ +int8_t KF_UpdateState(KF_t *kf) { + if (kf == NULL) return -1; + + if (!kf->skip_eq4) { + kf->temp_vector.numRows = kf->H.numRows; + kf->temp_vector.numCols = 1; + /* H·xhat'(k) */ + kf->mat_status = KF_MatMult(&kf->H, &kf->xhatminus, &kf->temp_vector); + kf->temp_vector1.numRows = kf->z.numRows; + kf->temp_vector1.numCols = 1; + /* 新息: z - H·xhat'(k) */ + kf->mat_status = KF_MatSub(&kf->z, &kf->temp_vector, &kf->temp_vector1); + kf->temp_vector.numRows = kf->K.numRows; + kf->temp_vector.numCols = 1; + /* K·新息 */ + kf->mat_status = KF_MatMult(&kf->K, &kf->temp_vector1, &kf->temp_vector); + /* xhat = xhat' + K·新息 */ + kf->mat_status = KF_MatAdd(&kf->xhatminus, &kf->temp_vector, &kf->xhat); + } + + return 0; +} + +/** + * @brief 步骤5：协方差更新 - P(k) = P'(k) - K·H·P'(k) + * + * @param kf 卡尔曼滤波器结构体指针 + * @return int8_t 0对应没有错误 + */ +int8_t KF_UpdateCovariance(KF_t *kf) { + if (kf == NULL) return -1; + + if (!kf->skip_eq5) { + 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; + /* K·H */ + kf->mat_status = KF_MatMult(&kf->K, &kf->H, &kf->temp_matrix); + /* K·H·P'(k) */ + kf->mat_status = KF_MatMult(&kf->temp_matrix, &kf->Pminus, &kf->temp_matrix1); + /* P = P' - K·H·P' */ + kf->mat_status = KF_MatSub(&kf->Pminus, &kf->temp_matrix1, &kf->P); + } + + return 0; +} + +/** + * @brief 执行完整的卡尔曼滤波周期（五大方程） + * + * 实现标准KF，并支持用户自定义函数钩子用于扩展（EKF/UKF/ESKF/AUKF）。 + * 每个步骤都可以通过 User_Func 回调函数替换。 + * + * @param kf 卡尔曼滤波器结构体指针 + * @return float* 滤波后的状态估计值指针 + */ +float *KF_Update(KF_t *kf) { + if (kf == NULL) return NULL; + + /* 步骤0: 量测获取和矩阵调整 */ + KF_Measure(kf); + if (kf->User_Func0_f != NULL) kf->User_Func0_f(kf); + + /* 步骤1: 先验状态估计 - xhat'(k) = F·xhat(k-1) + B·u */ + KF_PredictState(kf); + if (kf->User_Func1_f != NULL) kf->User_Func1_f(kf); + + /* 步骤2: 先验协方差 - P'(k) = F·P(k-1)·F^T + Q */ + KF_PredictCovariance(kf); + if (kf->User_Func2_f != NULL) kf->User_Func2_f(kf); + + /* 量测更新（仅在存在有效量测时执行） */ + if (kf->measurement_valid_num != 0 || kf->use_auto_adjustment == 0) { + /* 步骤3: 卡尔曼增益 - K = P'(k)·H^T / (H·P'(k)·H^T + R) */ + KF_CalcGain(kf); + if (kf->User_Func3_f != NULL) kf->User_Func3_f(kf); + + /* 步骤4: 状态更新 - xhat(k) = xhat'(k) + K·(z - H·xhat'(k)) */ + KF_UpdateState(kf); + if (kf->User_Func4_f != NULL) kf->User_Func4_f(kf); + + /* 步骤5: 协方差更新 - P(k) = P'(k) - K·H·P'(k) */ + KF_UpdateCovariance(kf); + } else { + /* 无有效量测 - 仅预测 */ + 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); + } + + /* 后处理钩子 */ + if (kf->User_Func5_f != NULL) kf->User_Func5_f(kf); + + /* 防过度收敛：强制最小方差 */ + for (uint8_t i = 0; i < kf->xhat_size; 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]; + } + + /* 复制结果到输出 */ + memcpy(kf->filtered_value, kf->xhat_data, sizeof(float) * kf->xhat_size); + + /* 附加后处理钩子 */ + if (kf->User_Func6_f != NULL) kf->User_Func6_f(kf); + + return kf->filtered_value; +} + +/** + * @brief 重置卡尔曼滤波器状态 + * + * @param kf 卡尔曼滤波器结构体指针 + */ +void KF_Reset(KF_t *kf) { + if (kf == NULL) return; + + memset(kf->xhat_data, 0, sizeof(float) * kf->xhat_size); + memset(kf->xhatminus_data, 0, sizeof(float) * kf->xhat_size); + memset(kf->filtered_value, 0, sizeof(float) * kf->xhat_size); + kf->measurement_valid_num = 0; +} + +/** + * @brief 根据量测有效性动态调整 H, R, K 矩阵 + * + * 该函数根据当前周期中哪些量测有效（非零）来重建量测相关矩阵。 + * 支持具有不同采样率的异步传感器。 + * + * @param kf 卡尔曼滤波器结构体指针 + */ +static void KF_AdjustHKR(KF_t *kf) { + kf->measurement_valid_num = 0; + + /* 复制并重置量测向量 */ + memcpy(kf->z_data, kf->measured_vector, sizeof(float) * kf->z_size); + memset(kf->measured_vector, 0, sizeof(float) * kf->z_size); + + /* 清空 H 和 R 矩阵 */ + 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); + + /* 识别有效量测并重建 z, H */ + for (uint8_t i = 0; i < kf->z_size; i++) { + if (kf->z_data[i] != 0) { /* 非零表示有效量测 */ + /* 将有效量测压缩到 z */ + kf->z_data[kf->measurement_valid_num] = kf->z_data[i]; + kf->temp[kf->measurement_valid_num] = i; + + /* 重建 H 矩阵行 */ + kf->H_data[kf->xhat_size * kf->measurement_valid_num + + kf->measurement_map[i] - 1] = kf->measurement_degree[i]; + kf->measurement_valid_num++; + } + } + + /* 用有效量测方差重建 R 矩阵 */ + for (uint8_t i = 0; i < kf->measurement_valid_num; i++) { + kf->R_data[i * kf->measurement_valid_num + i] = + kf->mat_r_diagonal_elements[kf->temp[i]]; + } + + /* 调整矩阵维度以匹配有效量测数量 */ + kf->H.numRows = kf->measurement_valid_num; + kf->H.numCols = kf->xhat_size; + kf->HT.numRows = kf->xhat_size; + kf->HT.numCols = kf->measurement_valid_num; + kf->R.numRows = kf->measurement_valid_num; + kf->R.numCols = kf->measurement_valid_num; + kf->K.numRows = kf->xhat_size; + kf->K.numCols = kf->measurement_valid_num; + kf->z.numRows = kf->measurement_valid_num; +} + +/* USER FUNCTION BEGIN */ + +/* USER FUNCTION END */ diff --git a/User/component/kalman_filter.h b/User/component/kalman_filter.h index bf54052..7e6b7d1 100644 --- a/User/component/kalman_filter.h +++ b/User/component/kalman_filter.h @@ -1,112 +1,199 @@ -/** - ****************************************************************************** - * @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 +/* + 卡尔曼滤波器 + 支持动态量测调整，使用ARM CMSIS DSP优化矩阵运算 +*/ +#pragma once + +#ifdef __cplusplus +extern "C" { +#endif #include "arm_math.h" -#include "math.h" -#include "stdint.h" -#include "stdlib.h" +#include +#include +#include +#include +/* USER INCLUDE BEGIN */ + +/* USER INCLUDE END */ + +/* USER DEFINE BEGIN */ + +/* USER DEFINE END */ + +/* 内存分配配置 */ #ifndef user_malloc #ifdef _CMSIS_OS_H -#define user_malloc pvPortMalloc +#define user_malloc pvPortMalloc /* FreeRTOS堆分配 */ #else -#define user_malloc malloc +#define user_malloc malloc /* 标准C库分配 */ #endif #endif -#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 +/* ARM CMSIS DSP 矩阵运算别名 */ +#define KF_Mat arm_matrix_instance_f32 +#define KF_MatInit arm_mat_init_f32 +#define KF_MatAdd arm_mat_add_f32 +#define KF_MatSub arm_mat_sub_f32 +#define KF_MatMult arm_mat_mult_f32 +#define KF_MatTrans arm_mat_trans_f32 +#define KF_MatInv arm_mat_inverse_f32 -typedef struct kf_t -{ - float *FilteredValue; - float *MeasuredVector; - float *ControlVector; +/* 卡尔曼滤波器主结构体 */ +typedef struct KF_s { + /* 输出和输入向量 */ + float *filtered_value; /* 滤波后的状态估计输出 */ + float *measured_vector; /* 量测输入向量 */ + float *control_vector; /* 控制输入向量 */ - uint8_t xhatSize; - uint8_t uSize; - uint8_t zSize; + /* 维度 */ + uint8_t xhat_size; /* 状态向量维度 */ + uint8_t u_size; /* 控制向量维度 */ + uint8_t z_size; /* 量测向量维度 */ - uint8_t UseAutoAdjustment; - uint8_t MeasurementValidNum; + /* 自动调整参数 */ + uint8_t use_auto_adjustment; /* 启用动态 H/R/K 调整 */ + uint8_t measurement_valid_num; /* 有效量测数量 */ - 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; + /* 量测配置 */ + uint8_t *measurement_map; /* 量测到状态的映射 */ + float *measurement_degree; /* 每个量测的H矩阵元素值 */ + float *mat_r_diagonal_elements; /* 量测噪声方差（R对角线） */ + float *state_min_variance; /* 最小状态方差（防过度收敛） */ + uint8_t *temp; /* 临时缓冲区 */ - // 配合用户定义函数使用,作为标志位用于判断是否要跳过标准KF中五个环节中的任意一个 - uint8_t SkipEq1, SkipEq2, SkipEq3, SkipEq4, SkipEq5; + /* 方程跳过标志（用于自定义用户函数） */ + uint8_t skip_eq1, skip_eq2, skip_eq3, skip_eq4, skip_eq5; - // 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,temp_F; // 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; + /* 卡尔曼滤波器矩阵 */ + KF_Mat xhat; /* 状态估计 x(k|k) */ + KF_Mat xhatminus; /* 先验状态估计 x(k|k-1) */ + KF_Mat u; /* 控制向量 */ + KF_Mat z; /* 量测向量 */ + KF_Mat P; /* 后验误差协方差 P(k|k) */ + KF_Mat Pminus; /* 先验误差协方差 P(k|k-1) */ + KF_Mat F, FT; /* 状态转移矩阵及其转置 */ + KF_Mat B; /* 控制矩阵 */ + KF_Mat H, HT; /* 量测矩阵及其转置 */ + KF_Mat Q; /* 过程噪声协方差 */ + KF_Mat R; /* 量测噪声协方差 */ + KF_Mat K; /* 卡尔曼增益 */ + KF_Mat S; /* 临时矩阵 S */ + KF_Mat temp_matrix, temp_matrix1; /* 临时矩阵 */ + KF_Mat temp_vector, temp_vector1; /* 临时向量 */ - int8_t MatStatus; + int8_t mat_status; /* 矩阵运算状态 */ - // 用户定义函数,可以替换或扩展基准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,*temp_F_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; + /* 用户自定义函数指针（用于EKF/UKF/ESKF扩展） */ + void (*User_Func0_f)(struct KF_s *kf); /* 自定义量测处理 */ + void (*User_Func1_f)(struct KF_s *kf); /* 自定义状态预测 */ + void (*User_Func2_f)(struct KF_s *kf); /* 自定义协方差预测 */ + void (*User_Func3_f)(struct KF_s *kf); /* 自定义卡尔曼增益计算 */ + void (*User_Func4_f)(struct KF_s *kf); /* 自定义状态更新 */ + void (*User_Func5_f)(struct KF_s *kf); /* 自定义后处理 */ + void (*User_Func6_f)(struct KF_s *kf); /* 附加自定义函数 */ -extern uint16_t sizeof_float, sizeof_double; + /* 矩阵数据存储指针 */ + 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; + float *temp_matrix_data, *temp_matrix_data1; + float *temp_vector_data, *temp_vector_data1; +} KF_t; -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); +/* USER STRUCT BEGIN */ -#endif //__KALMAN_FILTER_H +/* USER STRUCT END */ + +/** + * @brief 初始化卡尔曼滤波器并分配矩阵内存 + * + * @param kf 卡尔曼滤波器结构体指针 + * @param xhat_size 状态向量维度 + * @param u_size 控制向量维度（无控制输入时设为0） + * @param z_size 量测向量维度 + * @return int8_t 0对应没有错误 + */ +int8_t KF_Init(KF_t *kf, uint8_t xhat_size, uint8_t u_size, uint8_t z_size); + +/** + * @brief 获取量测并在启用自动调整时调整矩阵 + * + * @param kf 卡尔曼滤波器结构体指针 + * @return int8_t 0对应没有错误 + */ +int8_t KF_Measure(KF_t *kf); + +/** + * @brief 步骤1：先验状态估计 - xhat'(k) = F·xhat(k-1) + B·u + * + * @param kf 卡尔曼滤波器结构体指针 + * @return int8_t 0对应没有错误 + */ +int8_t KF_PredictState(KF_t *kf); + +/** + * @brief 步骤2：先验协方差 - P'(k) = F·P(k-1)·F^T + Q + * + * @param kf 卡尔曼滤波器结构体指针 + * @return int8_t 0对应没有错误 + */ +int8_t KF_PredictCovariance(KF_t *kf); + +/** + * @brief 步骤3：卡尔曼增益 - K = P'(k)·H^T / (H·P'(k)·H^T + R) + * + * @param kf 卡尔曼滤波器结构体指针 + * @return int8_t 0对应没有错误 + */ +int8_t KF_CalcGain(KF_t *kf); + +/** + * @brief 步骤4：状态更新 - xhat(k) = xhat'(k) + K·(z - H·xhat'(k)) + * + * @param kf 卡尔曼滤波器结构体指针 + * @return int8_t 0对应没有错误 + */ +int8_t KF_UpdateState(KF_t *kf); + +/** + * @brief 步骤5：协方差更新 - P(k) = P'(k) - K·H·P'(k) + * + * @param kf 卡尔曼滤波器结构体指针 + * @return int8_t 0对应没有错误 + */ +int8_t KF_UpdateCovariance(KF_t *kf); + +/** + * @brief 执行完整的卡尔曼滤波周期（五大方程） + * + * @param kf 卡尔曼滤波器结构体指针 + * @return float* 滤波后的状态估计值指针 + */ +float *KF_Update(KF_t *kf); + +/** + * @brief 重置卡尔曼滤波器状态 + * + * @param kf 卡尔曼滤波器结构体指针 + */ +void KF_Reset(KF_t *kf); + +/* USER FUNCTION BEGIN */ + +/* USER FUNCTION END */ + +#ifdef __cplusplus +} +#endif