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

897
User/component/kalman_filter.c
View File

@ -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);
/* 滤波数据分配 */
kf->filtered_value = (float *)user_malloc(sizeof(float) * xhat_size);
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
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);
/* 量测向量 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);
// 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);
/* 协方差矩阵 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);
// 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);
/* 先验协方差矩阵 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);
// 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);
/* 状态转移矩阵 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);
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);
/* 控制矩阵 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);
}
// 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);
/* 量测矩阵 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);
// 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);
/* 过程噪声协方差矩阵 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);
// 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);
/* 量测噪声协方差矩阵 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);
// 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);
/* 卡尔曼增益 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 * 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->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->SkipEq1 = 0;
kf->SkipEq2 = 0;
kf->SkipEq3 = 0;
kf->SkipEq4 = 0;
kf->SkipEq5 = 0;
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;
}
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);
/**
* @brief 获取量测并在启用自动调整时调整矩阵
*
* @param kf 卡尔曼滤波器结构体指针
* @return int8_t 0对应没有错误
*/
int8_t KF_Measure(KF_t *kf) {
if (kf == NULL) return -1;
/* 根据量测有效性自动调整 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)
{
if (!kf->SkipEq1)
{
if (kf->uSize > 0)
{
kf->temp_vector.numRows = kf->xhatSize;
/**
* @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;
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;
// 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->mat_status = KF_MatMult(&kf->F, &kf->xhat, &kf->temp_vector);
kf->temp_vector1.numRows = kf->xhat_size;
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);
}
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);
}
}
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);
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;
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);
/* 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;
}
void Kalman_Filter_SetK(KalmanFilter_t *kf)
{
if (!kf->SkipEq3)
{
kf->MatStatus = Matrix_Transpose(&kf->H, &kf->HT); // z|x => x|z
/**
* @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;
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.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.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)
/* 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;
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);
/* 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;
}
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.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.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.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);
/* 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;
}
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.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·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 执行卡尔曼滤波黄金五式,提供了用户定义函数,可以替代五个中的任意一个环节,方便自行扩展为EKF/UKF/ESKF/AUKF等
* @brief 执行完整的卡尔曼滤波周期（五大方程）
*
* @param kf kf类型定义
* @return float* 返回滤波值
* 实现标准KF，并支持用户自定义函数钩子用于扩展（EKF/UKF/ESKF/AUKF）。
* 每个步骤都可以通过 User_Func 回调函数替换。
*
* @param 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);
float *KF_Update(KF_t *kf) {
if (kf == NULL) return NULL;
// 先验估计
// 1. xhat'(k)= A·xhat(k-1) + B·u
Kalman_Filter_xhatMinusUpdate(kf);
if (kf->User_Func1_f != NULL)
kf->User_Func1_f(kf);
/* 步骤0: 量测获取和矩阵调整 */
KF_Measure(kf);
if (kf->User_Func0_f != NULL) kf->User_Func0_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);
/* 步骤1: 先验状态估计 - xhat'(k) = F·xhat(k-1) + B·u */
KF_PredictState(kf);
if (kf->User_Func1_f != NULL) kf->User_Func1_f(kf);
if (kf->MeasurementValidNum != 0 || kf->UseAutoAdjustment == 0)
{
// 量测更新
// 3. K(k) = P'(k)·HT / (H·P'(k)·HT + R)
Kalman_Filter_SetK(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->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(k)·(z(k) - H·xhat'(k))
Kalman_Filter_xhatUpdate(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);
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);
/* 步骤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);
/* 后处理钩子 */
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];
/* 防过度收敛：强制最小方差 */
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->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)
{
kf->MeasurementValidNum = 0;
/**
* @brief 重置卡尔曼滤波器状态
*
* @param kf 卡尔曼滤波器结构体指针
*/
void KF_Reset(KF_t *kf) {
if (kf == NULL) return;
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]];
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;
}
// 调整矩阵维数
// 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 */

245
User/component/kalman_filter.h
View File

@ -1,112 +1,199 @@
/**
******************************************************************************
* @file kalman filter.h
* @author Wang Hongxi
* @version V1.2.2
* @date 2022/1/8
* @brief
******************************************************************************
* @attention
*
******************************************************************************
/*
卡尔曼滤波器
支持动态量测调整，使用ARM CMSIS DSP优化矩阵运算
*/
#ifndef __KALMAN_FILTER_H
#define __KALMAN_FILTER_H
#pragma once
#ifdef __cplusplus
extern "C" {
#endif
#include "arm_math.h"
#include "math.h"
#include "stdint.h"
#include "stdlib.h"
#include <math.h>
#include <stdint.h>
#include <stdlib.h>
#include <string.h>
/* 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);
/* 用户自定义函数指针（用于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); /* 附加自定义函数 */
// 矩阵存储空间指针
/* 矩阵数据存储指针 */
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 *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;
float *S_data;
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);
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 END */
#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