cbb/Motor_v2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

feat(FOC): 添加电流卡尔曼滤波器并优化MIT控制模式

Browse Source 
- 优化MIT控制模式的角度和速度计算
- 调整多个PID控制器的参数配置
- 增强卡尔曼滤波器的数值稳定性
This commit is contained in:
cbb
2026-02-08 18:53:55 +08:00
parent 173fadadd2
commit 9689841148
10 changed files with 237 additions and 58 deletions
Download Patch File Download Diff File Expand all files Collapse all files
Robot_V2/Algorithm/FOC.cpp
+22 -16
View File
@@ -16,6 +16,7 @@ extern sampler sampler;
extern angle_sensor KTH7823;
extern CAN_transmiter CAN0_transmiter;
extern EncoderVelocityKalman Vel_kalman; // 速度滤波
extern CurrentKalman Current_kalman;
// 初始变量及函数定义
#define _constrain(amt, low, high) \
@@ -29,8 +30,8 @@ inline float FOC_Controller::cal_Iq_Id(float current_a, float current_b,
// 计算电气角度
float angle_el = fmod(shaft_angle * PP, 360.0f);
float I_alpha, I_beta;
// Clarke变换 (使用三相电流)
float I_alpha, I_beta;
I_alpha = (2.0f / 3.0f) * current_a - (1.0f / 3.0f) * current_b -
(1.0f / 3.0f) * current_c;
I_beta = _1_SQRT3 * current_b - _1_SQRT3 * current_c;
@@ -41,8 +42,8 @@ inline float FOC_Controller::cal_Iq_Id(float current_a, float current_b,
float I_q, I_d;
I_q = I_beta * cos_el - I_alpha * sin_el;
return I_q * 1000.0f -
KE_FACTOR * rt_velocity; // 返回Iq值，乘以1000以转换为mA
// 返回Iq值，乘以1000以转换为mA
return Current_kalman.update(I_q * 1000 - KE_FACTOR * rt_velocity);
}
/**
@@ -52,12 +53,15 @@ inline float FOC_Controller::cal_Iq_Id(float current_a, float current_b,
*/
u8 FOC_Controller::init() {
motor_init(); // 电机驱动初始化
vel_loop.set_PID(DEFAULT_vel_loop_p * 100 / voltage_power_supply, vel_loop_Ki,
vel_loop.set_PID(DEFAULT_vel_loop_p * 100 / voltage_power_supply,
vel_loop_Ki,
vel_loop_Kd); // 设置电机速度控制参数
angle_loop.set_PID(DEFAULT_angle_loop_p * 100 / voltage_power_supply, 100.0f,
angle_loop.set_PID(DEFAULT_angle_loop_p * 100 / voltage_power_supply,
100.0f,
0.2f); // 设置电机角度控制参数
current_loop.set_PID(DEFAULT_current_loop_p * 100 / voltage_power_supply, 5e-7f,
7e-8); // 设置电机电流控制参数
current_loop.set_PID(DEFAULT_current_loop_p * 100 / voltage_power_supply,
current_loop_Ki,
current_loop_Kd); // 设置电机电流控制参数
vel_inVelAngloop.set_PID(DEFAULT_vel_angloop_p * 100, vel_angloop_Ki,
vel_angloop_Kd); // 设置电机速度控制参数
@@ -65,7 +69,9 @@ u8 FOC_Controller::init() {
ang_angloop_Ki,
ang_angloop_Kd); // 设置电机角度控制参数
CAN0_transmiter.testfloat_union.f = DEFAULT_ang_angloop_p * 100 / voltage_power_supply;
vel_inMITloop.set_PID(DEFAULT_ang_angloop_p * 100 / voltage_power_supply,
vel_MIT_Ki,
vel_MIT_Kd); // 设置电机角度控制参数
return 0;
}
@@ -262,7 +268,7 @@ void FOC_Controller::update_power(float voltage) {
voltage_error = false;
voltage_power_supply = voltage;
v_half = voltage * 0.5f;
v_safe = voltage * 1.05f;
v_safe = voltage * 1.1f;
} else {
voltage_error = true;
}
@@ -347,7 +353,7 @@ float FOC_Controller::angle_control_with_velocity_limit() {
}
/**
* @brief 带速度限制的角度闭环控制
* @brief MIT控制
*
* @param target_angle: 目标角度
* @param max_velocity: 最大速度限制 (单位: rad/s)
@@ -356,17 +362,17 @@ float FOC_Controller::angle_control_with_velocity_limit() {
float FOC_Controller::MIT_Mode() {
// 第一层：角度环（外环）
// 根据目标角度和实际角度的差值，通过角度环PID控制器计算目标速度
float angle_error = rt_angle - target_angle;
float angle_error = target_angle - rt_angle;
float target_velocity =
ang_inVelAngloop(angle_error); // 使用angle_loop作为角度到速度的映射
CAN0_transmiter.testfloat_union.f = target_velocity;
// 第二层：速度环（中环）
// 根据目标速度和实际速度的差值，通过速度环PID控制器计算目标电流
float velocity_error = target_velocity - rt_velocity;
float target_current =
vel_inVelAngloop(velocity_error); // 使用vel_loop作为速度到电流的映射
float velocity_error = rt_velocity - target_velocity;
vel_inMITloop.set_P(vel_MIT_p);
float target_current = vel_inMITloop(velocity_error);
CAN0_transmiter.testfloat_union.f = target_current;
// 第三层：电流环（内环）
// 计算当前实际电流
rt_current = cal_Iq_Id(sampler.get_current('U'), sampler.get_current('V'),
@@ -410,8 +416,8 @@ float FOC_Controller::current_control() {
float Iq = rt_current =
cal_Iq_Id(sampler.get_current('U'), sampler.get_current('V'),
sampler.get_current('W'));
current_loop.set_P(current_loop_p);
float Uq = current_loop(target_current - Iq);
CAN0_transmiter.testfloat_union.f = Iq;
// 获取电机轴角度
shaft_angle = KTH7823.getcurrent_sensorangle();
// 设置相电压
Robot_V2/Algorithm/FOC.h
+8 -2
View File
@@ -64,9 +64,15 @@ class FOC_Controller {
PIDController current_loop = PIDController{current_loop_p * 100, 0, 0, 1500,
voltage_power_supply}; // 12V
PIDController vel_inVelAngloop =
PIDController{vel_angloop_p * 100, 0, 0, 1000000, v_safe};
PIDController{vel_angloop_p * 100, 0, 0, 1e6, v_safe};
PIDController ang_inVelAngloop =
PIDController{ang_angloop_p * 100, 0, 0, 20000, 100};
PIDController{ang_angloop_p * 100, 0, 0, 1e4, 100};
PIDController ang_inMITloop =
PIDController{ang_angloop_p * 100, 0, 0, 1e4, 100};
PIDController vel_inMITloop =
PIDController{vel_MIT_p * 100, 0, 0, 1e9, 6000};
void update_pid_limits() {
vel_loop.limit = voltage_power_supply;
Robot_V2/Algorithm/Filter.cpp
+75 -7
View File
@@ -8,8 +8,20 @@
extern angle_sensor KTH7823;
extern CAN_transmiter CAN0_transmiter;
extern u32 motor_runtime_50us;
EncoderAngleKalman Angle_kalman(0, 1.0f, 1e-4f, 0.25f);
EncoderVelocityKalman Vel_kalman(0, 1e-5f, 1e-6f, 0.02f);
// 使用配置文件中的参数
EncoderAngleKalman Angle_kalman(ANGLE_KALMAN_INITIAL_STATE,
ANGLE_KALMAN_INITIAL_COVARIANCE,
ANGLE_KALMAN_PROCESS_NOISE_Q,
ANGLE_KALMAN_MEASUREMENT_NOISE_R);
EncoderVelocityKalman Vel_kalman(VELOCITY_KALMAN_INITIAL_STATE,
VELOCITY_KALMAN_INITIAL_COVARIANCE,
VELOCITY_KALMAN_PROCESS_NOISE_Q,
VELOCITY_KALMAN_MEASUREMENT_NOISE_R);
CurrentKalman Current_kalman( CURRENT_KALMAN_INITIAL_STATE, CURRENT_KALMAN_INITIAL_COVARIANCE,
CURRENT_KALMAN_PROCESS_NOISE_Q,
CURRENT_KALMAN_MEASUREMENT_NOISE_R);
/*!
\brief 中值滤波器函数
@@ -96,7 +108,9 @@ EncoderAngleKalman::EncoderAngleKalman(float initial_angle,
float measurement_noise)
: kalman(initial_angle, initial_error_covariance, process_noise,
measurement_noise),
last_angle(initial_angle), initialized(true) {}
last_angle(initial_angle), initialized(true),
velocity_last_angle(initial_angle),
velocity_last_us50_counter(motor_runtime_50us) {}
/*!
\brief 更新卡尔曼滤波器状态
@@ -125,10 +139,18 @@ float EncoderAngleKalman::getAngle() const { return kalman.getState(); }
\retval 当前角度估计值 (degree)
*/
float EncoderAngleKalman::getVelocity() const {
static u32 last_us50_counter = motor_runtime_50us;
float vel = (kalman.getState() - last_angle) * TIMER2_FREQUENCY *
(motor_runtime_50us - last_us50_counter);
last_us50_counter = motor_runtime_50us;
float angle_now = kalman.getState();
u32 time_counts = motor_runtime_50us - velocity_last_us50_counter;
float vel = 0.0f;
if (time_counts > 0) {
vel = (angle_now - velocity_last_angle) * TIMER2_FREQUENCY /
time_counts * 2;
}
velocity_last_angle = angle_now;
velocity_last_us50_counter = motor_runtime_50us;
return vel;
}
@@ -146,4 +168,50 @@ void EncoderAngleKalman::setProcessNoise(float q) { kalman.setProcessNoise(q); }
*/
void EncoderAngleKalman::setMeasurementNoise(float r) {
kalman.setMeasurementNoise(r);
}
// CurrentKalman类的实现
/*!
\brief 构造函数，初始化电流卡尔曼滤波器参数
\param[in] initial_current 初始电流值 (A)
\param[in] initial_error_covariance 初始误差协方差
\param[in] process_noise 过程噪声协方差
\param[in] measurement_noise 测量噪声协方差
\retval none
*/
CurrentKalman::CurrentKalman(float initial_current,
float initial_error_covariance,
float process_noise, float measurement_noise)
: kalman(initial_current, initial_error_covariance, process_noise,
measurement_noise) {}
/*!
\brief 更新卡尔曼滤波器状态
\param[in] measured_current 测量的电流值 (A)
\retval 估计的电流值 (A)
*/
float CurrentKalman::update(float measured_current) {
return kalman.update(measured_current);
}
/*!
\brief 获取当前电流估计值
\retval 当前电流估计值 (A)
*/
float CurrentKalman::getCurrent() const { return kalman.getState(); }
/*!
\brief 设置过程噪声协方差
\param[in] q 过程噪声协方差值
\retval none
*/
void CurrentKalman::setProcessNoise(float q) { kalman.setProcessNoise(q); }
/*!
\brief 设置测量噪声协方差
\param[in] r 测量噪声协方差值
\retval none
*/
void CurrentKalman::setMeasurementNoise(float r) {
kalman.setMeasurementNoise(r);
}
Robot_V2/Algorithm/Filter.h
+73 -8
View File
@@ -5,16 +5,17 @@
extern "C" {
#endif
// C风格函数声明可以放在这里
// 例如: void some_c_function(float* data, int size);
#ifdef __cplusplus
} // extern "C"
#endif
#ifdef __cplusplus
#include <cstring>
#include "Can.h"
#include <cstdint>
#include <cstring>
extern CAN_transmiter CAN0_transmiter;
float medium_filter(float values[], u8 size);
@@ -75,10 +76,19 @@ template <typename T> class KalmanFilter {
\retval 估计的状态值
*/
T update(T measurement) {
T K = P / (P + R); // 卡尔曼增益
T K = P / (P + R); // 卡尔曼增益
x += K * (measurement - x); // 更新状态估计
P = (1 - K) * P; // 更新误差协方差
P = (1 - K) * P; // 更新误差协方差
// 添加协方差下限保护，防止数值不稳定
const T MIN_COVARIANCE = static_cast<T>(1e-6);
if (P < MIN_COVARIANCE) {
P = MIN_COVARIANCE;
}
return x;
}
@@ -101,6 +111,25 @@ template <typename T> class KalmanFilter {
\retval none
*/
void setMeasurementNoise(T measurement_noise) { R = measurement_noise; }
/*!
\brief 重置估计误差协方差
\param[in] error_covariance 误差协方差值
\retval none
*/
void resetCovariance(T error_covariance) { P = error_covariance; }
/*!
\brief 获取当前误差协方差
\retval 当前误差协方差
*/
T getCovariance() const { return P; }
/*!
\brief 获取当前过程噪声协方差
\retval 当前过程噪声协方差
*/
T getProcessNoise() const { return Q; }
};
/*!
@@ -124,8 +153,9 @@ class EncoderVelocityKalman {
void setMeasurementNoise(float r);
u8 init(float init_velocity) {
u8 init(float init_velocity, float initial_error_covariance = 1.0f) {
kalman.x = init_velocity;
kalman.resetCovariance(initial_error_covariance); // 重置估计误差协方差
return 0;
}
};
@@ -139,6 +169,9 @@ class EncoderAngleKalman {
KalmanFilter<float> kalman;
bool initialized;
float last_angle;
// 添加成员变量来替代静态变量
mutable u32 velocity_last_us50_counter;
mutable float velocity_last_angle;
public:
EncoderAngleKalman(float initial_angle, float initial_error_covariance,
@@ -153,12 +186,44 @@ class EncoderAngleKalman {
float getstate() const { return kalman.getState(); };
void setMeasurementNoise(float r);
u8 init(float init_angle) {
u8 init(float init_angle, float initial_error_covariance = 1.0f) {
kalman.x = init_angle;
kalman.resetCovariance(initial_error_covariance); // 重置估计误差协方差
last_angle = init_angle; // 同时重置last_angle
// 重置速度计算相关的变量
velocity_last_angle = init_angle;
extern u32 motor_runtime_50us;
velocity_last_us50_counter = motor_runtime_50us;
return 0;
}
};
/*!
\brief 电流卡尔曼滤波器类
\note 用于滤除电流测量噪声的卡尔曼滤波器实现
*/
class CurrentKalman {
private:
KalmanFilter<float> kalman;
public:
CurrentKalman(float initial_current, float initial_error_covariance,
float process_noise, float measurement_noise);
float update(float measured_current);
float getCurrent() const;
void setProcessNoise(float q);
void setMeasurementNoise(float r);
u8 init(float init_current, float initial_error_covariance = 1.0f) {
kalman.x = init_current;
kalman.resetCovariance(initial_error_covariance); // 重置估计误差协方差
return 0;
}
};
#endif // __cplusplus
#endif // _FILTER_H
Robot_V2/device/ADC.cpp
+10 -6
View File
@@ -5,9 +5,9 @@
#include "config.h"
#include "math.h"
#include "systick.h"
#include <cstdint>
#include <cstring>
#include <gd32c10x_dma.h>
#include <cstdint>
constexpr float ADC_SCALE = 3.3f / 4096.0f;
constexpr float TEMP_FACTOR = 1000.0f / 4.3f;
@@ -16,6 +16,7 @@ constexpr float CURRENT_FACTOR =
1.0f / 50.0f * 100.0f; // 50倍放大，100倍电流采样电阻, 单位mA
extern CAN_transmiter CAN0_transmiter;
extern FOC_Controller FOC;
sampler sampler; // 采样器初始化
u16 adc_value[6] = {0}; // 显式初始化数组元素为0
/*!
@@ -167,20 +168,25 @@ float sampler::get_drv_temp() {
* 通过ADC采样计算各相电流值
*/
float sampler::get_current(char type) {
u8 idx = current_sample_index;
switch (type) {
case 'U': {
// return currU_samples[current_sample_index];
tempora_sample = medium_filter(currU_samples, ADC_FILTER_SIZE + 1);
// tempora_sample = medium_filter(currU_samples, ADC_FILTER_SIZE + 1);
tempora_sample = currU_samples[idx];
break;
}
case 'V': {
// return currV_samples[current_sample_index];
tempora_sample = medium_filter(currV_samples, ADC_FILTER_SIZE + 1);
// tempora_sample = medium_filter(currV_samples, ADC_FILTER_SIZE + 1);
tempora_sample = currV_samples[idx];
break;
}
case 'W': {
// return currW_samples[current_sample_index];
tempora_sample = medium_filter(currW_samples, ADC_FILTER_SIZE + 1);
// tempora_sample = medium_filter(currW_samples, ADC_FILTER_SIZE + 1);
tempora_sample = currW_samples[idx];
break;
}
default: {
@@ -254,8 +260,6 @@ void sampler::update_current() {
if (current_sample_index >= ADC_FILTER_SIZE - 1) {
current_sample_index = 0;
}
// 卡尔曼滤波更新
float voltage = FOC.voltage_power_supply;
}
/**
Robot_V2/device/Can.cpp
+2 -1
View File
@@ -537,6 +537,7 @@ void CAN_transmiter::communication_check() {
target_current_union.f =
unpack_bytes_to_float(receive_message.rx_data, 3).f;
FOC.current_loop.set_limit(target_current_union.f);
FOC.vel_inMITloop.set_limit(target_current_union.f);
// 目标速度
target_velocity_union.f =
unpack_bytes_to_float(receive_message.rx_data, 7).f;
@@ -557,7 +558,6 @@ void CAN_transmiter::communication_check() {
P_config = unpack_bytes_to_u16(receive_message.rx_data, 17);
FOC.vel_loop_p = DEFAULT_vel_loop_p * P_config / voltage;
FOC.vel_MIT_p = DEFAULT_vel_MIT_p * P_config / voltage;
P_config = unpack_bytes_to_u16(receive_message.rx_data, 19);
FOC.angle_loop_p = DEFAULT_angle_loop_p * P_config / voltage;
@@ -565,6 +565,7 @@ void CAN_transmiter::communication_check() {
P_config = unpack_bytes_to_u16(receive_message.rx_data, 21);
FOC.vel_angloop_p = DEFAULT_vel_angloop_p * P_config;
FOC.vel_MIT_p = DEFAULT_vel_MIT_p * P_config;
} else if (receive_message.rx_efid == CANFD_RESET_ID) {
u8 first_byte = receive_message.rx_data[0];
Robot_V2/device/Timer.cpp
+1 -1
View File
@@ -96,7 +96,6 @@ void TIMER2_IRQHandler(void) {
// 每50us执行一次的操作 - 电机控制
scheduler.taskFlags |=
(1 << TASK_ANGLE_UPDATE) | (1 << TASK_VELOCITY_UPDATE);
scheduler.taskFlags |= (1 << TASK_MOTOR_CONTROL);
// 使用标志位控制，每100us执行一次其他任务
static bool isEvenCall = false;
@@ -105,6 +104,7 @@ void TIMER2_IRQHandler(void) {
if (isEvenCall) {
// 每100us执行一次的操作 - 保持原有频率
scheduler.taskFlags |= (1 << TASK_MOTOR_CONTROL);
switch (total_us100_counter) { // 每1ms执行的代码，负载均衡
case 0: {
scheduler.taskFlags |= (1 << TASK_MCU_TEMP_UPDATE);
Robot_V2/device/ang_sensor.cpp
-1
View File
@@ -265,7 +265,6 @@ float angle_sensor::update(void) {
m_cumulativeAngle += delta;
m_lastAngle = current_angle_deg;
return m_cumulativeAngle;
}
Robot_V2/main.cpp
+2 -2
View File
@@ -22,8 +22,8 @@ extern VersionManager Version;
// 全局版本管理器实例
TaskScheduler scheduler = {0};
volatile u32 motor_runtime_50us = 0; // 电机驱动计算轮次
volatile u32 cnt_test = 0; // 负载检测
u32 motor_runtime_50us = 0; // 电机驱动计算轮次
u32 cnt_test = 0; // 负载检测
int main(void) {
u8 init_status = hardware_init();
Robot_V2/sys/config.h
+44 -14
View File
@@ -12,9 +12,9 @@
// 版本号定义 - 可根据实际项目进行调整
constexpr u16 SOFTWARE_VERSION_MAJOR = 26;
constexpr u16 SOFTWARE_VERSION_MINOR = 1;
constexpr u16 SOFTWARE_VERSION_PATCH = 24;
constexpr u16 SOFTWARE_VERSION_BUILD = 1;
constexpr u16 SOFTWARE_VERSION_MINOR = 2;
constexpr u16 SOFTWARE_VERSION_PATCH = 8;
constexpr u16 SOFTWARE_VERSION_BUILD = 5;
constexpr u16 HARDWARE_VERSION_MAJOR = 25;
constexpr u16 HARDWARE_VERSION_MINOR = 12;
@@ -57,9 +57,11 @@ constexpr u16 REDUCTION_RATIO = 25;
constexpr float CURRENT_OFFSET = -1.62f; // 当前电流传感器的零点偏移
constexpr float HOLD_PWM_DUTY = 0.95f;
constexpr u16 ANGLESENS_OFFSET = +0;
constexpr float KE_FACTOR = 14.0f;
constexpr float KE_FACTOR = 0.0f;
// PID配置
constexpr float DEFAULT_current_loop_p = 6e-2f / 100 * 12; // 电流P
constexpr float DEFAULT_current_loop_p = 0.1f / 100 * 12; // 电流P
constexpr float current_loop_Ki = 0.55f; // 电流P
constexpr float current_loop_Kd = 3e-4; // 电流P
constexpr float DEFAULT_vel_loop_p = 0.018f / 100 * 12; // 速度P
constexpr float vel_loop_Ku = 3.600000e-03 * 0.7 * 100 / 12;
@@ -67,21 +69,49 @@ constexpr float vel_loop_Tu = 17.0f * 1e-3f;
constexpr float vel_loop_Ki = 1.2 * vel_loop_Ku / vel_loop_Tu;
constexpr float vel_loop_Kd = 0.075 * vel_loop_Ku * vel_loop_Tu * 2;
constexpr float DEFAULT_vel_angloop_p = 0.22f / 100; // 速度 速度-角度P
constexpr float vel_angloop_Ki = 12.0f; // 角度 速度-角度I
constexpr float vel_angloop_Kd = 6e-04f; // 角度 速度-角度D
constexpr float DEFAULT_vel_angloop_p = 0.4f / 100; // 速度 速度-角度P
constexpr float vel_angloop_Ki = 8.0f; // 速度 速度-角度I
constexpr float vel_angloop_Kd = 5e-05f; // 速度 速度-角度D
constexpr float DEFAULT_ang_angloop_p = 120.0f / 100 * 12; // 角度 速度-角度P
constexpr float ang_angloop_Ki = 0.00f; // 角度 速度-角度I
constexpr float ang_angloop_Kd = 8e-04; // 角度 速度-角度D
constexpr float DEFAULT_ang_angloop_p = 150.0f / 100 * 12; // 角度 速度-角度P
constexpr float ang_angloop_Ki = 0.00f; // 角度 速度-角度I
constexpr float ang_angloop_Kd = 8e-04; // 角度 速度-角度D
constexpr float DEFAULT_angle_loop_p = 60.0f / 100 * 12; // 角度P
constexpr float DEFAULT_vel_MIT_p = 25.0f / 100 * 12; // 速度 速度-角度P
constexpr float DEFAULT_vel_MIT_p = 9.0f / 100; // 速度 MIT P
constexpr float vel_MIT_Ku = 6.72f * 100 / 12;
constexpr float vel_MIT_Tu = 62.0f * 1e-3f;
constexpr float vel_MIT_Ki = 3e-2f;
constexpr float vel_MIT_Kd = 5.355000e-05f;
constexpr float vel_MIT_Ki = 0.1f;
constexpr float vel_MIT_Kd = 0.01f;
// 角度卡尔曼滤波器参数
// 参数调节建议：
// - 增大Q值：增加对测量的信任，提高响应速度
// - 减小Q值：增加对预测的信任，提高平滑度
// - 增大R值：增加平滑度，减少噪声，但会降低响应速度
// - 减小R值：提高响应速度，但会增加噪声
constexpr float ANGLE_KALMAN_INITIAL_STATE = 0.0f;
constexpr float ANGLE_KALMAN_INITIAL_COVARIANCE = 0.0f;
constexpr float ANGLE_KALMAN_PROCESS_NOISE_Q =
1e-3f; // 过程噪声协方差 - 调节此参数控制平滑度
constexpr float ANGLE_KALMAN_MEASUREMENT_NOISE_R =
0.0062f; // 测量噪声协方差 - 调节此参数控制响应速度
// 速度卡尔曼滤波器参数
constexpr float VELOCITY_KALMAN_INITIAL_STATE = 0.0f;
constexpr float VELOCITY_KALMAN_INITIAL_COVARIANCE = 1e-5f;
constexpr float VELOCITY_KALMAN_PROCESS_NOISE_Q =
1e-6f; // 过程噪声协方差 - 通常保持较小
constexpr float VELOCITY_KALMAN_MEASUREMENT_NOISE_R =
1.0f; // 测量噪声协方差 - 主要调节参数
// 电流卡尔曼滤波器参数
constexpr float CURRENT_KALMAN_INITIAL_STATE = 0.0f;
constexpr float CURRENT_KALMAN_INITIAL_COVARIANCE = 100.0f; // 增大初始协方差
constexpr float CURRENT_KALMAN_PROCESS_NOISE_Q = 10.0f; // 降低过程噪声
constexpr float CURRENT_KALMAN_MEASUREMENT_NOISE_R = 1e-3f; // 增大测量噪声
// 常量定义
constexpr u8 SIZE_FLOAT = sizeof(float);
constexpr u8 SIZE_U32 = sizeof(uint32_t);