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

989
User/component/kalman_filter.c
View File

File diff suppressed because it is too large Load Diff

265
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