添加卡尔曼滤波器
This commit is contained in:
parent
6d73319b54
commit
be86c08ef4
File diff suppressed because it is too large
Load Diff
@ -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 <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 *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 *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;
|
||||
|
||||
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
|
||||
|
||||
Loading…
Reference in New Issue
Block a user