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

添加卡尔曼滤波器

Browse Source
This commit is contained in:
Robofish 2026-02-02 02:46:09 +08:00
parent 6d73319b54
commit be86c08ef4
2 changed files with 724 additions and 534 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

901
User/component/kalman_filter.c
View File

@ -1,488 +1,591 @@
/** /*
****************************************************************************** 卡尔曼滤波器 modified from wang hongxi
* @file kalman filter.c 支持动态量测调整，使用ARM CMSIS DSP优化矩阵运算
* @author Wang Hongxi
* @version V1.2.2 主要特性：
* @date 2022/1/8 - 基于量测有效性的 H、R、K 矩阵动态调整
* @brief C implementation of kalman filter - 支持不同传感器采样频率
****************************************************************************** - 矩阵 P 防过度收敛机制
* @attention - ARM CMSIS DSP 优化的矩阵运算
* 该卡尔曼滤波器可以在传感器采样频率不同的情况下，动态调整矩阵H R和K的维数与数值。 - 可扩展架构，支持用户自定义函数（EKF/UKF/ESKF）
* 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. 1. 矩阵初始化：P、F、Q 使用标准初始化方式
* H、R 在使用自动调整时需要配置量测映射
* 因此矩阵H和R的初始化会与矩阵P A和Q有所不同。另外的，在初始化量测向量z时需要额外写
* 入传感器量测所对应的状态与这个量测的方式，详情请见例程 2. 自动调整模式 (use_auto_adjustment = 1)：
* Therefore, the initialization of matrix P, F, and Q is sometimes different - 提供 measurement_map：每个量测对应的状态索引
* from that of matrices H R. when initialization. Additionally, the corresponding - 提供 measurement_degree：H 矩阵元素值
* state and the method of the measurement should be provided when initializing - 提供 mat_r_diagonal_elements：量测噪声方差
* measurement vector z. For more details, please see the example.
* 3. 固定模式 (use_auto_adjustment = 0)：
* 若不需要动态调整量测向量z，可简单将结构体中的Use_Auto_Adjustment初始化为0，并像初 - 像初始化 P 矩阵那样正常初始化 z、H、R
* 始化矩阵P那样用常规方式初始化z H R即可。
* If automatic adjustment is not required, assign zero to the UseAutoAdjustment 4. 量测更新：
* and initialize z H R in the normal way as matrix P. - 在传感器回调函数中更新 measured_vector
* - 值为 0 表示量测无效
* 要求量测向量z与控制向量u在传感器回调函数中更新。整数0意味着量测无效，即自上次卡尔曼 - 向量在每次 KF 更新后会被重置为 0
* 滤波更新后无传感器数据更新。因此量测向量z与控制向量u会在卡尔曼滤波更新过程中被清零
* MeasuredVector and ControlVector are required to be updated in the sensor 5. 防过度收敛：
* callback function. Integer 0 in measurement vector z indicates the invalidity - 设置 state_min_variance 防止 P 矩阵过度收敛
* of current measurement, so MeasuredVector and ControlVector will be reset - 帮助滤波器适应缓慢变化的状态
* (to 0) during each update.
* 使用示例：高度估计
* 此外，矩阵P过度收敛后滤波器将难以再适应状态的缓慢变化，从而产生滤波估计偏差。该算法 状态向量 x =
* 通过限制矩阵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. KF_t Height_KF;
*
* @example: void INS_Task_Init(void)
* x = {
* | height | // 初始协方差矩阵 P
* | velocity | static float P_Init[9] =
* |acceleration| {
* 10, 0, 0,
* KalmanFilter_t Height_KF; 0, 30, 0,
* 0, 0, 10,
* void INS_Task_Init(void) };
* {
* static float P_Init[9] = // 状态转移矩阵 F（根据运动学模型）
* { static float F_Init[9] =
* 10, 0, 0, {
* 0, 30, 0, 1, dt, 0.5*dt*dt,
* 0, 0, 10, 0, 1, dt,
* }; 0, 0, 1,
* static float F_Init[9] = };
* {
* 1, dt, 0.5*dt*dt, // 过程噪声协方差矩阵 Q
* 0, 1, dt, static float Q_Init[9] =
* 0, 0, 1, {
* }; 0.25*dt*dt*dt*dt, 0.5*dt*dt*dt, 0.5*dt*dt,
* static float Q_Init[9] = 0.5*dt*dt*dt, dt*dt, dt,
* { 0.5*dt*dt, dt, 1,
* 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};
*
* // 设置最小方差 // 开启自动调整
* static float state_min_variance[3] = {0.03, 0.005, 0.1}; Height_KF.use_auto_adjustment = 1;
*
* // 开启自动调整 // 量测映射：[气压高度对应状态1, GPS高度对应状态1, 加速度计对应状态3]
* Height_KF.UseAutoAdjustment = 1; static uint8_t measurement_reference[3] = {1, 1, 3};
*
* // 气压测得高度 GPS测得高度 加速度计测得z轴运动加速度 // 量测系数（H矩阵元素值）
* static uint8_t measurement_reference[3] = {1, 1, 3} static float measurement_degree[3] = {1, 1, 1};
* // 根据 measurement_reference 与 measurement_degree 生成 H 矩阵如下
* static float measurement_degree[3] = {1, 1, 1} // （在当前周期全部量测数据有效的情况下）
* // 根据measurement_reference与measurement_degree生成H矩阵如下（在当前周期全部测量数据有效情况下） // |1 0 0|
* |1 0 0| // |1 0 0|
* |1 0 0| // |0 0 1|
* |0 0 1|
* // 量测噪声方差（R矩阵对角元素）
* static float mat_R_diagonal_elements = {30, 25, 35} static float mat_r_diagonal_elements[3] = {30, 25, 35};
* //根据mat_R_diagonal_elements生成R矩阵如下（在当前周期全部测量数据有效情况下） // 根据 mat_r_diagonal_elements 生成 R 矩阵如下
* |30 0 0| // （在当前周期全部量测数据有效的情况下）
* | 0 25 0| // |30 0 0|
* | 0 0 35| // | 0 25 0|
* // | 0 0 35|
* Kalman_Filter_Init(&Height_KF, 3, 0, 3);
* // 初始化卡尔曼滤波器（状态维数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.P_data, P_Init, sizeof(P_Init));
* memcpy(Height_KF.MeasurementMap, measurement_reference, sizeof(measurement_reference)); memcpy(Height_KF.F_data, F_Init, sizeof(F_Init));
* memcpy(Height_KF.MeasurementDegree, measurement_degree, sizeof(measurement_degree)); memcpy(Height_KF.Q_data, Q_Init, sizeof(Q_Init));
* memcpy(Height_KF.MatR_DiagonalElements, mat_R_diagonal_elements, sizeof(mat_R_diagonal_elements)); memcpy(Height_KF.measurement_map, measurement_reference,
* memcpy(Height_KF.StateMinVariance, state_min_variance, sizeof(state_min_variance)); sizeof(measurement_reference));
* } memcpy(Height_KF.measurement_degree, measurement_degree,
* sizeof(measurement_degree));
* void INS_Task(void const *pvParameters) 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,
* Kalman_Filter_Update(&Height_KF); sizeof(state_min_variance));
* vTaskDelay(ts); }
* }
* void INS_Task(void const *pvParameters)
* // 测量数据更新应按照以下形式 即向MeasuredVector赋值 {
* void Barometer_Read_Over(void) // 循环更新卡尔曼滤波器
* { KF_Update(&Height_KF);
* ...... vTaskDelay(ts);
* INS_KF.MeasuredVector[0] = baro_height; }
* }
* void GPS_Read_Over(void) // 传感器回调函数示例：在数据就绪时更新 measured_vector
* { void Barometer_Read_Over(void)
* ...... {
* INS_KF.MeasuredVector[1] = GPS_height; ......
* } INS_KF.measured_vector[0] = baro_height; // 气压计高度
* void Acc_Data_Process(void) }
* {
* ...... void GPS_Read_Over(void)
* INS_KF.MeasuredVector[2] = acc.z; {
* } ......
****************************************************************************** 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" #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 kf 卡尔曼滤波器结构体指针
* @param xhatSize 状态变量维度 * @param xhat_size 状态向量维度
* @param uSize 控制变量维度 * @param u_size 控制向量维度（无控制输入时设为0）
* @param zSize 观测量维度 * @param z_size 量测向量维度
* @return int8_t 0对应没有错误
*/ */
void Kalman_Filter_Init(KalmanFilter_t *kf, uint8_t xhatSize, uint8_t uSize, uint8_t zSize) int8_t KF_Init(KF_t *kf, uint8_t xhat_size, uint8_t u_size, uint8_t z_size) {
{ if (kf == NULL) return -1;
sizeof_float = sizeof(float);
sizeof_double = sizeof(double);
kf->xhatSize = xhatSize; kf->xhat_size = xhat_size;
kf->uSize = uSize; kf->u_size = u_size;
kf->zSize = zSize; 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); kf->measurement_map = (uint8_t *)user_malloc(sizeof(uint8_t) * z_size);
memset(kf->MeasurementMap, 0, sizeof(uint8_t) * zSize); memset(kf->measurement_map, 0, sizeof(uint8_t) * z_size);
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->measurement_degree = (float *)user_malloc(sizeof(float) * z_size);
kf->FilteredValue = (float *)user_malloc(sizeof_float * xhatSize); memset(kf->measurement_degree, 0, sizeof(float) * z_size);
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->mat_r_diagonal_elements = (float *)user_malloc(sizeof(float) * z_size);
kf->xhat_data = (float *)user_malloc(sizeof_float * xhatSize); memset(kf->mat_r_diagonal_elements, 0, sizeof(float) * z_size);
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->state_min_variance = (float *)user_malloc(sizeof(float) * xhat_size);
kf->xhatminus_data = (float *)user_malloc(sizeof_float * xhatSize); memset(kf->state_min_variance, 0, sizeof(float) * xhat_size);
memset(kf->xhatminus_data, 0, sizeof_float * xhatSize);
Matrix_Init(&kf->xhatminus, kf->xhatSize, 1, (float *)kf->xhatminus_data);
if (uSize != 0) kf->temp = (uint8_t *)user_malloc(sizeof(uint8_t) * z_size);
{ memset(kf->temp, 0, sizeof(uint8_t) * z_size);
// control vector u
kf->u_data = (float *)user_malloc(sizeof_float * uSize); /* 滤波数据分配 */
memset(kf->u_data, 0, sizeof_float * uSize); kf->filtered_value = (float *)user_malloc(sizeof(float) * xhat_size);
Matrix_Init(&kf->u, kf->uSize, 1, (float *)kf->u_data); memset(kf->filtered_value, 0, sizeof(float) * xhat_size);
kf->measured_vector = (float *)user_malloc(sizeof(float) * z_size);
memset(kf->measured_vector, 0, sizeof(float) * z_size);
kf->control_vector = (float *)user_malloc(sizeof(float) * u_size);
memset(kf->control_vector, 0, sizeof(float) * u_size);
/* 状态估计 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);
/* 先验状态估计 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);
/* 控制向量 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);
} }
// measurement vector z /* 量测向量 z */
kf->z_data = (float *)user_malloc(sizeof_float * zSize); kf->z_data = (float *)user_malloc(sizeof(float) * z_size);
memset(kf->z_data, 0, sizeof_float * zSize); memset(kf->z_data, 0, sizeof(float) * z_size);
Matrix_Init(&kf->z, kf->zSize, 1, (float *)kf->z_data); KF_MatInit(&kf->z, kf->z_size, 1, kf->z_data);
// covariance matrix P(k|k) /* 协方差矩阵 P(k|k) */
kf->P_data = (float *)user_malloc(sizeof_float * xhatSize * xhatSize); kf->P_data = (float *)user_malloc(sizeof(float) * xhat_size * xhat_size);
memset(kf->P_data, 0, sizeof_float * xhatSize * xhatSize); memset(kf->P_data, 0, sizeof(float) * xhat_size * xhat_size);
Matrix_Init(&kf->P, kf->xhatSize, kf->xhatSize, (float *)kf->P_data); KF_MatInit(&kf->P, kf->xhat_size, kf->xhat_size, kf->P_data);
// create covariance matrix P(k|k-1) /* 先验协方差矩阵 P(k|k-1) */
kf->Pminus_data = (float *)user_malloc(sizeof_float * xhatSize * xhatSize); kf->Pminus_data = (float *)user_malloc(sizeof(float) * xhat_size * xhat_size);
memset(kf->Pminus_data, 0, sizeof_float * xhatSize * xhatSize); memset(kf->Pminus_data, 0, sizeof(float) * xhat_size * xhat_size);
Matrix_Init(&kf->Pminus, kf->xhatSize, kf->xhatSize, (float *)kf->Pminus_data); KF_MatInit(&kf->Pminus, kf->xhat_size, kf->xhat_size, kf->Pminus_data);
// state transition matrix F FT /* 状态转移矩阵 F 及其转置 FT */
kf->F_data = (float *)user_malloc(sizeof_float * xhatSize * xhatSize); kf->F_data = (float *)user_malloc(sizeof(float) * xhat_size * xhat_size);
kf->FT_data = (float *)user_malloc(sizeof_float * xhatSize * xhatSize); kf->FT_data = (float *)user_malloc(sizeof(float) * xhat_size * xhat_size);
memset(kf->F_data, 0, sizeof_float * xhatSize * xhatSize); memset(kf->F_data, 0, sizeof(float) * xhat_size * xhat_size);
memset(kf->FT_data, 0, sizeof_float * xhatSize * xhatSize); memset(kf->FT_data, 0, sizeof(float) * xhat_size * xhat_size);
Matrix_Init(&kf->F, kf->xhatSize, kf->xhatSize, (float *)kf->F_data); KF_MatInit(&kf->F, kf->xhat_size, kf->xhat_size, kf->F_data);
Matrix_Init(&kf->FT, kf->xhatSize, kf->xhatSize, (float *)kf->FT_data); KF_MatInit(&kf->FT, kf->xhat_size, kf->xhat_size, kf->FT_data);
if (uSize != 0) /* 控制矩阵 B */
{ if (u_size != 0) {
// control matrix B kf->B_data = (float *)user_malloc(sizeof(float) * xhat_size * u_size);
kf->B_data = (float *)user_malloc(sizeof_float * xhatSize * uSize); memset(kf->B_data, 0, sizeof(float) * xhat_size * u_size);
memset(kf->B_data, 0, sizeof_float * xhatSize * uSize); KF_MatInit(&kf->B, kf->xhat_size, kf->u_size, kf->B_data);
Matrix_Init(&kf->B, kf->xhatSize, kf->uSize, (float *)kf->B_data);
} }
// measurement matrix H /* 量测矩阵 H 及其转置 HT */
kf->H_data = (float *)user_malloc(sizeof_float * zSize * xhatSize); kf->H_data = (float *)user_malloc(sizeof(float) * z_size * xhat_size);
kf->HT_data = (float *)user_malloc(sizeof_float * xhatSize * zSize); kf->HT_data = (float *)user_malloc(sizeof(float) * xhat_size * z_size);
memset(kf->H_data, 0, sizeof_float * zSize * xhatSize); memset(kf->H_data, 0, sizeof(float) * z_size * xhat_size);
memset(kf->HT_data, 0, sizeof_float * xhatSize * zSize); memset(kf->HT_data, 0, sizeof(float) * xhat_size * z_size);
Matrix_Init(&kf->H, kf->zSize, kf->xhatSize, (float *)kf->H_data); KF_MatInit(&kf->H, kf->z_size, kf->xhat_size, kf->H_data);
Matrix_Init(&kf->HT, kf->xhatSize, kf->zSize, (float *)kf->HT_data); KF_MatInit(&kf->HT, kf->xhat_size, kf->z_size, kf->HT_data);
// process noise covariance matrix Q /* 过程噪声协方差矩阵 Q */
kf->Q_data = (float *)user_malloc(sizeof_float * xhatSize * xhatSize); kf->Q_data = (float *)user_malloc(sizeof(float) * xhat_size * xhat_size);
memset(kf->Q_data, 0, sizeof_float * xhatSize * xhatSize); memset(kf->Q_data, 0, sizeof(float) * xhat_size * xhat_size);
Matrix_Init(&kf->Q, kf->xhatSize, kf->xhatSize, (float *)kf->Q_data); KF_MatInit(&kf->Q, kf->xhat_size, kf->xhat_size, kf->Q_data);
// measurement noise covariance matrix R /* 量测噪声协方差矩阵 R */
kf->R_data = (float *)user_malloc(sizeof_float * zSize * zSize); kf->R_data = (float *)user_malloc(sizeof(float) * z_size * z_size);
memset(kf->R_data, 0, sizeof_float * zSize * zSize); memset(kf->R_data, 0, sizeof(float) * z_size * z_size);
Matrix_Init(&kf->R, kf->zSize, kf->zSize, (float *)kf->R_data); KF_MatInit(&kf->R, kf->z_size, kf->z_size, kf->R_data);
// kalman gain K /* 卡尔曼增益 K */
kf->K_data = (float *)user_malloc(sizeof_float * xhatSize * zSize); kf->K_data = (float *)user_malloc(sizeof(float) * xhat_size * z_size);
memset(kf->K_data, 0, sizeof_float * xhatSize * zSize); memset(kf->K_data, 0, sizeof(float) * xhat_size * z_size);
Matrix_Init(&kf->K, kf->xhatSize, kf->zSize, (float *)kf->K_data); KF_MatInit(&kf->K, kf->xhat_size, kf->z_size, 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->S_data = (float *)user_malloc(sizeof(float) * xhat_size * xhat_size);
kf->temp_matrix_data1 = (float *)user_malloc(sizeof_float * kf->xhatSize * kf->xhatSize); kf->temp_matrix_data =
kf->temp_vector_data = (float *)user_malloc(sizeof_float * kf->xhatSize); (float *)user_malloc(sizeof(float) * xhat_size * xhat_size);
kf->temp_vector_data1 = (float *)user_malloc(sizeof_float * kf->xhatSize); kf->temp_matrix_data1 =
Matrix_Init(&kf->S, kf->xhatSize, kf->xhatSize, (float *)kf->S_data); (float *)user_malloc(sizeof(float) * xhat_size * xhat_size);
Matrix_Init(&kf->temp_matrix, kf->xhatSize, kf->xhatSize, (float *)kf->temp_matrix_data); kf->temp_vector_data = (float *)user_malloc(sizeof(float) * xhat_size);
Matrix_Init(&kf->temp_matrix1, kf->xhatSize, kf->xhatSize, (float *)kf->temp_matrix_data1); kf->temp_vector_data1 = (float *)user_malloc(sizeof(float) * xhat_size);
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_MatInit(&kf->S, kf->xhat_size, kf->xhat_size, kf->S_data);
kf->SkipEq2 = 0; KF_MatInit(&kf->temp_matrix, kf->xhat_size, kf->xhat_size,
kf->SkipEq3 = 0; kf->temp_matrix_data);
kf->SkipEq4 = 0; KF_MatInit(&kf->temp_matrix1, kf->xhat_size, kf->xhat_size,
kf->SkipEq5 = 0; 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;
} }
void Kalman_Filter_Measure(KalmanFilter_t *kf) /**
{ * @brief 获取量测并在启用自动调整时调整矩阵
// 矩阵H K R根据量测情况自动调整 *
// matrix H K R auto adjustment * @param kf 卡尔曼滤波器结构体指针
if (kf->UseAutoAdjustment != 0) * @return int8_t 0对应没有错误
H_K_R_Adjustment(kf); */
else int8_t KF_Measure(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); /* 根据量测有效性自动调整 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);
} }
memcpy(kf->u_data, kf->ControlVector, sizeof_float * kf->uSize); memcpy(kf->u_data, kf->control_vector, sizeof(float) * kf->u_size);
return 0;
} }
extern int stop_time; /**
void Kalman_Filter_xhatMinusUpdate(KalmanFilter_t *kf) * @brief 步骤1：先验状态估计 - xhat'(k) = F·xhat(k-1) + B·u
{ *
if (!kf->SkipEq1) * @param kf 卡尔曼滤波器结构体指针
{ * @return int8_t 0对应没有错误
if (kf->uSize > 0) */
{ int8_t KF_PredictState(KF_t *kf) {
kf->temp_vector.numRows = kf->xhatSize; if (kf == NULL) return -1;
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->temp_vector.numCols = 1;
// if(stop_time==0) kf->mat_status = KF_MatMult(&kf->F, &kf->xhat, &kf->temp_vector);
// {
// kf->MatStatus = Matrix_Multiply(&kf->temp_F, &kf->xhat, &kf->temp_vector); kf->temp_vector1.numRows = kf->xhat_size;
// }
// else
// {
kf->MatStatus = Matrix_Multiply(&kf->F, &kf->xhat, &kf->temp_vector);
// }
kf->temp_vector1.numRows = kf->xhatSize;
kf->temp_vector1.numCols = 1; kf->temp_vector1.numCols = 1;
kf->MatStatus = Matrix_Multiply(&kf->B, &kf->u, &kf->temp_vector1); kf->mat_status = KF_MatMult(&kf->B, &kf->u, &kf->temp_vector1);
kf->MatStatus = Matrix_Add(&kf->temp_vector, &kf->temp_vector1, &kf->xhatminus); kf->mat_status =
} KF_MatAdd(&kf->temp_vector, &kf->temp_vector1, &kf->xhatminus);
else } else {
{ /* 无控制输入: xhat'(k) = F·xhat(k-1) */
kf->MatStatus = Matrix_Multiply(&kf->F, &kf->xhat, &kf->xhatminus); kf->mat_status = KF_MatMult(&kf->F, &kf->xhat, &kf->xhatminus);
} }
} }
return 0;
} }
void Kalman_Filter_PminusUpdate(KalmanFilter_t *kf) /**
{ * @brief 步骤2：先验协方差 - P'(k) = F·P(k-1)·F^T + Q
if (!kf->SkipEq2) *
{ * @param kf 卡尔曼滤波器结构体指针
kf->MatStatus = Matrix_Transpose(&kf->F, &kf->FT); * @return int8_t 0对应没有错误
kf->MatStatus = Matrix_Multiply(&kf->F, &kf->P, &kf->Pminus); */
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.numRows = kf->Pminus.numRows;
kf->temp_matrix.numCols = kf->FT.numCols; 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 /* F·P(k-1)·F^T */
kf->MatStatus = Matrix_Add(&kf->temp_matrix, &kf->Q, &kf->Pminus); 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;
} }
void Kalman_Filter_SetK(KalmanFilter_t *kf)
{ /**
if (!kf->SkipEq3) * @brief 步骤3：卡尔曼增益 - K = P'(k)·H^T / (H·P'(k)·H^T + R)
{ *
kf->MatStatus = Matrix_Transpose(&kf->H, &kf->HT); // z|x => x|z * @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.numRows = kf->H.numRows;
kf->temp_matrix.numCols = kf->Pminus.numCols; kf->temp_matrix.numCols = kf->Pminus.numCols;
kf->MatStatus = Matrix_Multiply(&kf->H, &kf->Pminus, &kf->temp_matrix); // temp_matrix = H·P'(k) /* 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.numRows = kf->temp_matrix.numRows;
kf->temp_matrix1.numCols = kf->HT.numCols; 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 /* 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.numRows = kf->R.numRows;
kf->S.numCols = kf->R.numCols; kf->S.numCols = kf->R.numCols;
kf->MatStatus = Matrix_Add(&kf->temp_matrix1, &kf->R, &kf->S); // S = H P'(k) HT + R /* S = H·P'(k)·H^T + R */
kf->MatStatus = Matrix_Inverse(&kf->S, &kf->temp_matrix1); // temp_matrix1 = inv(H·P'(k)·HT + 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.numRows = kf->Pminus.numRows;
kf->temp_matrix.numCols = kf->HT.numCols; kf->temp_matrix.numCols = kf->HT.numCols;
kf->MatStatus = Matrix_Multiply(&kf->Pminus, &kf->HT, &kf->temp_matrix); // temp_matrix = P'(k)·HT /* P'(k)·H^T */
kf->MatStatus = Matrix_Multiply(&kf->temp_matrix, &kf->temp_matrix1, &kf->K); 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;
} }
void Kalman_Filter_xhatUpdate(KalmanFilter_t *kf)
{ /**
if (!kf->SkipEq4) * @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.numRows = kf->H.numRows;
kf->temp_vector.numCols = 1; kf->temp_vector.numCols = 1;
kf->MatStatus = Matrix_Multiply(&kf->H, &kf->xhatminus, &kf->temp_vector); // temp_vector = H xhat'(k) /* 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.numRows = kf->z.numRows;
kf->temp_vector1.numCols = 1; kf->temp_vector1.numCols = 1;
kf->MatStatus = Matrix_Subtract(&kf->z, &kf->temp_vector, &kf->temp_vector1); // temp_vector1 = z(k) - H·xhat'(k) /* 新息: 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.numRows = kf->K.numRows;
kf->temp_vector.numCols = 1; 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)) /* K·新息 */
kf->MatStatus = Matrix_Add(&kf->xhatminus, &kf->temp_vector, &kf->xhat); 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;
} }
void Kalman_Filter_P_Update(KalmanFilter_t *kf)
{ /**
if (!kf->SkipEq5) * @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.numRows = kf->K.numRows;
kf->temp_matrix.numCols = kf->H.numCols; kf->temp_matrix.numCols = kf->H.numCols;
kf->temp_matrix1.numRows = kf->temp_matrix.numRows; kf->temp_matrix1.numRows = kf->temp_matrix.numRows;
kf->temp_matrix1.numCols = kf->Pminus.numCols; kf->temp_matrix1.numCols = kf->Pminus.numCols;
kf->MatStatus = Matrix_Multiply(&kf->K, &kf->H, &kf->temp_matrix); // temp_matrix = K(k)·H /* K·H */
kf->MatStatus = Matrix_Multiply(&kf->temp_matrix, &kf->Pminus, &kf->temp_matrix1); // temp_matrix1 = K(k)·H·P'(k) kf->mat_status = KF_MatMult(&kf->K, &kf->H, &kf->temp_matrix);
kf->MatStatus = Matrix_Subtract(&kf->Pminus, &kf->temp_matrix1, &kf->P); /* 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 执行卡尔曼滤波黄金五式,提供了用户定义函数,可以替代五个中的任意一个环节,方便自行扩展为EKF/UKF/ESKF/AUKF等 * @brief 执行完整的卡尔曼滤波周期（五大方程）
* *
* @param kf kf类型定义 * 实现标准KF，并支持用户自定义函数钩子用于扩展（EKF/UKF/ESKF/AUKF）。
* @return float* 返回滤波值 * 每个步骤都可以通过 User_Func 回调函数替换。
*
* @param kf 卡尔曼滤波器结构体指针
* @return float* 滤波后的状态估计值指针
*/ */
float *Kalman_Filter_Update(KalmanFilter_t *kf) float *KF_Update(KF_t *kf) {
{ if (kf == NULL) return NULL;
// 0. 获取量测信息
Kalman_Filter_Measure(kf);
if (kf->User_Func0_f != NULL)
kf->User_Func0_f(kf);
// 先验估计 /* 步骤0: 量测获取和矩阵调整 */
// 1. xhat'(k)= A·xhat(k-1) + B·u KF_Measure(kf);
Kalman_Filter_xhatMinusUpdate(kf); if (kf->User_Func0_f != NULL) kf->User_Func0_f(kf);
if (kf->User_Func1_f != NULL)
kf->User_Func1_f(kf);
// 预测更新 /* 步骤1: 先验状态估计 - xhat'(k) = F·xhat(k-1) + B·u */
// 2. P'(k) = A·P(k-1)·AT + Q KF_PredictState(kf);
Kalman_Filter_PminusUpdate(kf); if (kf->User_Func1_f != NULL) kf->User_Func1_f(kf);
if (kf->User_Func2_f != NULL)
kf->User_Func2_f(kf);
if (kf->MeasurementValidNum != 0 || kf->UseAutoAdjustment == 0) /* 步骤2: 先验协方差 - P'(k) = F·P(k-1)·F^T + Q */
{ KF_PredictCovariance(kf);
// 量测更新 if (kf->User_Func2_f != NULL) kf->User_Func2_f(kf);
// 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); 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)) */
// 4. xhat(k) = xhat'(k) + K(k)·(z(k) - H·xhat'(k)) KF_UpdateState(kf);
Kalman_Filter_xhatUpdate(kf); if (kf->User_Func4_f != NULL) kf->User_Func4_f(kf);
if (kf->User_Func4_f != NULL) /* 步骤5: 协方差更新 - P(k) = P'(k) - K·H·P'(k) */
kf->User_Func4_f(kf); KF_UpdateCovariance(kf);
} else {
// 修正方差 /* 无有效量测 - 仅预测 */
// 5. P(k) = (1-K(k)·H)·P'(k) ==> P(k) = P'(k)-K(k)·H·P'(k) memcpy(kf->xhat_data, kf->xhatminus_data, sizeof(float) * kf->xhat_size);
Kalman_Filter_P_Update(kf); memcpy(kf->P_data, kf->Pminus_data,
} sizeof(float) * kf->xhat_size * kf->xhat_size);
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) if (kf->User_Func5_f != NULL) kf->User_Func5_f(kf);
kf->User_Func5_f(kf);
// 避免滤波器过度收敛 /* 防过度收敛：强制最小方差 */
// suppress filter excessive convergence for (uint8_t i = 0; i < kf->xhat_size; i++) {
for (uint8_t i = 0; i < kf->xhatSize; 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];
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); /* 复制结果到输出 */
memcpy(kf->filtered_value, kf->xhat_data, sizeof(float) * kf->xhat_size);
if (kf->User_Func6_f != NULL) /* 附加后处理钩子 */
kf->User_Func6_f(kf); if (kf->User_Func6_f != NULL) kf->User_Func6_f(kf);
return kf->FilteredValue; return kf->filtered_value;
} }
static void H_K_R_Adjustment(KalmanFilter_t *kf) /**
{ * @brief 重置卡尔曼滤波器状态
kf->MeasurementValidNum = 0; *
* @param kf 卡尔曼滤波器结构体指针
*/
void KF_Reset(KF_t *kf) {
if (kf == NULL) return;
memcpy(kf->z_data, kf->MeasuredVector, sizeof_float * kf->zSize); memset(kf->xhat_data, 0, sizeof(float) * kf->xhat_size);
memset(kf->MeasuredVector, 0, sizeof_float * kf->zSize); memset(kf->xhatminus_data, 0, sizeof(float) * kf->xhat_size);
memset(kf->filtered_value, 0, sizeof(float) * kf->xhat_size);
// 识别量测数据有效性并调整矩阵H R K kf->measurement_valid_num = 0;
// 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;
} }
/**
* @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 */

247
User/component/kalman_filter.h
View File

@ -1,112 +1,199 @@
/** /*
****************************************************************************** 卡尔曼滤波器
* @file kalman filter.h 支持动态量测调整，使用ARM CMSIS DSP优化矩阵运算
* @author Wang Hongxi */
* @version V1.2.2
* @date 2022/1/8
* @brief
******************************************************************************
* @attention
*
******************************************************************************
*/
#ifndef __KALMAN_FILTER_H
#define __KALMAN_FILTER_H
#pragma once
#ifdef __cplusplus
extern "C" {
#endif
#include "arm_math.h" #include "arm_math.h"
#include "math.h" #include <math.h>
#include "stdint.h" #include <stdint.h>
#include "stdlib.h" #include <stdlib.h>
#include <string.h>
/* USER INCLUDE BEGIN */
/* USER INCLUDE END */
/* USER DEFINE BEGIN */
/* USER DEFINE END */
/* 内存分配配置 */
#ifndef user_malloc #ifndef user_malloc
#ifdef _CMSIS_OS_H #ifdef _CMSIS_OS_H
#define user_malloc pvPortMalloc #define user_malloc pvPortMalloc /* FreeRTOS堆分配 */
#else #else
#define user_malloc malloc #define user_malloc malloc /* 标准C库分配 */
#endif #endif
#endif #endif
#define mat arm_matrix_instance_f32 /* ARM CMSIS DSP 矩阵运算别名 */
#define Matrix_Init arm_mat_init_f32 #define KF_Mat arm_matrix_instance_f32
#define Matrix_Add arm_mat_add_f32 #define KF_MatInit arm_mat_init_f32
#define Matrix_Subtract arm_mat_sub_f32 #define KF_MatAdd arm_mat_add_f32
#define Matrix_Multiply arm_mat_mult_f32 #define KF_MatSub arm_mat_sub_f32
#define Matrix_Transpose arm_mat_trans_f32 #define KF_MatMult arm_mat_mult_f32
#define Matrix_Inverse arm_mat_inverse_f32 #define KF_MatTrans arm_mat_trans_f32
#define KF_MatInv arm_mat_inverse_f32
typedef struct kf_t /* 卡尔曼滤波器主结构体 */
{ typedef struct KF_s {
float *FilteredValue; /* 输出和输入向量 */
float *MeasuredVector; float *filtered_value; /* 滤波后的状态估计输出 */
float *ControlVector; float *measured_vector; /* 量测输入向量 */
float *control_vector; /* 控制输入向量 */
uint8_t xhatSize; /* 维度 */
uint8_t uSize; uint8_t xhat_size; /* 状态向量维度 */
uint8_t zSize; 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 uint8_t *measurement_map; /* 量测到状态的映射 */
float *MatR_DiagonalElements; // 量测方差 variance for each measurement float *measurement_degree; /* 每个量测的H矩阵元素值 */
float *StateMinVariance; // 最小方差 避免方差过度收敛 suppress filter excessive convergence float *mat_r_diagonal_elements; /* 量测噪声方差（R对角线） */
uint8_t *temp; 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) KF_Mat xhat; /* 状态估计 x(k|k) */
mat xhatminus; // x(k|k-1) KF_Mat xhatminus; /* 先验状态估计 x(k|k-1) */
mat u; // control vector u KF_Mat u; /* 控制向量 */
mat z; // measurement vector z KF_Mat z; /* 量测向量 */
mat P; // covariance matrix P(k|k) KF_Mat P; /* 后验误差协方差 P(k|k) */
mat Pminus; // covariance matrix P(k|k-1) KF_Mat Pminus; /* 先验误差协方差 P(k|k-1) */
mat F, FT,temp_F; // state transition matrix F FT KF_Mat F, FT; /* 状态转移矩阵及其转置 */
mat B; // control matrix B KF_Mat B; /* 控制矩阵 */
mat H, HT; // measurement matrix H KF_Mat H, HT; /* 量测矩阵及其转置 */
mat Q; // process noise covariance matrix Q KF_Mat Q; /* 过程噪声协方差 */
mat R; // measurement noise covariance matrix R KF_Mat R; /* 量测噪声协方差 */
mat K; // kalman gain K KF_Mat K; /* 卡尔曼增益 */
mat S, temp_matrix, temp_matrix1, temp_vector, temp_vector1; KF_Mat S; /* 临时矩阵 S */
KF_Mat temp_matrix, temp_matrix1; /* 临时矩阵 */
KF_Mat temp_vector, temp_vector1; /* 临时向量 */
int8_t MatStatus; int8_t mat_status; /* 矩阵运算状态 */
// 用户定义函数,可以替换或扩展基准KF的功能 /* 用户自定义函数指针（用于EKF/UKF/ESKF扩展） */
void (*User_Func0_f)(struct kf_t *kf); void (*User_Func0_f)(struct KF_s *kf); /* 自定义量测处理 */
void (*User_Func1_f)(struct kf_t *kf); void (*User_Func1_f)(struct KF_s *kf); /* 自定义状态预测 */
void (*User_Func2_f)(struct kf_t *kf); void (*User_Func2_f)(struct KF_s *kf); /* 自定义协方差预测 */
void (*User_Func3_f)(struct kf_t *kf); void (*User_Func3_f)(struct KF_s *kf); /* 自定义卡尔曼增益计算 */
void (*User_Func4_f)(struct kf_t *kf); void (*User_Func4_f)(struct KF_s *kf); /* 自定义状态更新 */
void (*User_Func5_f)(struct kf_t *kf); void (*User_Func5_f)(struct KF_s *kf); /* 自定义后处理 */
void (*User_Func6_f)(struct kf_t *kf); void (*User_Func6_f)(struct KF_s *kf); /* 附加自定义函数 */
// 矩阵存储空间指针 /* 矩阵数据存储指针 */
float *xhat_data, *xhatminus_data; float *xhat_data, *xhatminus_data;
float *u_data; float *u_data;
float *z_data; float *z_data;
float *P_data, *Pminus_data; float *P_data, *Pminus_data;
float *F_data, *FT_data,*temp_F_data; float *F_data, *FT_data;
float *B_data; float *B_data;
float *H_data, *HT_data; float *H_data, *HT_data;
float *Q_data; float *Q_data;
float *R_data; float *R_data;
float *K_data; float *K_data;
float *S_data, *temp_matrix_data, *temp_matrix_data1, *temp_vector_data, *temp_vector_data1; float *S_data;
} KalmanFilter_t; float *temp_matrix_data, *temp_matrix_data1;
float *temp_vector_data, *temp_vector_data1;
} KF_t;
extern uint16_t sizeof_float, sizeof_double; /* USER STRUCT BEGIN */
void Kalman_Filter_Init(KalmanFilter_t *kf, uint8_t xhatSize, uint8_t uSize, uint8_t zSize); /* USER STRUCT END */
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 /**
* @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