初始化

2025-09-23 00:27:51 +08:00
commit f2ae2669fa
75 changed files with 32841 additions and 0 deletions
@@ -0,0 +1,124 @@
+#include "Filter.h"
+#include "stdbool.h"
+#include <algorithm>
+#include <array>
+#include <cstring>
+#include <stdint.h>
+
+//队列实现
+void CircularBuffer::enqueue(const float value) {
+	buffer[head] = value;
+	head = (head + 1) % HISTORY_SIZE;
+	if (count < HISTORY_SIZE)
+		count++;
+}
+
+/*!
+	\brief 			获取操作 - O(1)
+	\param[out] 	float 最新的元素
+	\retval 		最新的元素值
+	\note 			注意：此操作不删除元素，只是返回最新的元素
+*/
+float CircularBuffer::get(size_t index) const {
+	if (index >= count)
+		return buffer[head]; // 返回最新元素
+	size_t pos = (head + HISTORY_SIZE - 1 - index) % HISTORY_SIZE;
+	return buffer[pos];
+}
+
+/*!
+	\brief      获取最新的5个数值
+	\param[out] latest_values: 指向存储最新5个数值的数组的指针
+	\retval     none
+	\note       数组必须至少能容纳5个float元素
+*/
+void CircularBuffer::getLatestFive(float *latest_values) const {
+	for (size_t i = 0; i < 5 && i < count; ++i) {
+		latest_values[i] = get(i);
+	}
+
+	// 如果历史数据不足5个，用同样的数字填充剩余位置
+	for (size_t i = count; i < 5; ++i) {
+		latest_values[i] = latest_values[count - 1];
+	}
+}
+
+/*!
+ \brief 	中值滤波器函数
+ \param[in] values 待滤波的数据数组
+ \param[in] size 数组大小
+ \retval    滤波后的中值
+ \note      使用部分排序优化算法提高效率
+*/
+float medium_filter(float values[], u8 size) {
+	// 使用局部数组避免动态内存分配
+	float sorted[HISTORY_SIZE];
+	memcpy(sorted, values, size * sizeof(float));
+
+	// 这比完整排序更高效，时间复杂度从O(n log n)降低到O(n)
+	std::nth_element(sorted, sorted + size / 2, sorted + size);
+
+	return sorted[size / 2]; // 返回中值
+}
+
+// CurrentKalman类的实现
+/*!
+	\brief      构造函数，初始化卡尔曼滤波器参数
+	\param[in]  initial_current 初始电流值
+	\param[in]  L 电机电感(H)	0.0026H
+	\param[in]  R 电机电阻(Ω)	4.8 Ω
+	\param[in]  T 控制周期(s)	1e -4
+	\param[in]  Q 过程噪声协方差(典型值: 1e-6 ~ 1e-4)
+	\param[in]  R_noise 测量噪声协方差(典型值: 电流范围的0.5%~2%)
+	\retval     none
+*/
+CurrentKalman::CurrentKalman(float initial_current, float L, float R, float T,
+							 float Q, float R_noise)
+	: x(initial_current), P(1.0f), // 初始误差协方差
+	  Q(Q),						   // 过程噪声
+	  R(R_noise),				   // 测量噪声
+	  L(L), R_m(R), T(T) {}
+/*!
+	\brief      更新卡尔曼滤波器状态
+	\param[in]  measured_current 测量电流值
+	\param[in]  applied_voltage 应用电压值
+	\retval     估计的电流值
+	\note       实现卡尔曼滤波的预测和更新步骤
+*/
+float CurrentKalman::update(float measured_current, float applied_voltage) {
+	// 1. 预测步骤 - 基于电机模型
+	float predicted_current = x + T * (applied_voltage - R_m * x) / L;
+
+	// 2. 计算预测误差协方差
+	float P_pred = P + Q;
+
+	// 3. 计算卡尔曼增益
+	float K = P_pred / (P_pred + R);
+
+	// 4. 更新状态估计
+	x = predicted_current + K * (measured_current - predicted_current);
+
+	// 5. 更新估计误差协方差
+	P = (1 - K) * P_pred;
+
+	return x;
+}
+/*!
+	\brief      获取当前电流估计值
+	\retval     当前电流估计值
+*/
+float CurrentKalman::getCurrent() const { return x; }
+/*!
+	\brief      设置过程噪声协方差
+	\param[in]  q 过程噪声协方差值
+	\retval     none
+*/
+void CurrentKalman::setProcessNoise(float q) { Q = q; }
+/*!
+	\brief      设置测量噪声协方差
+	\param[in]  r 测量噪声协方差值
+	\retval     none
+*/
+void CurrentKalman::setMeasurementNoise(float r) { R = r; }
+
+
@@ -0,0 +1,74 @@
+#ifndef _FILTER_H
+#define _FILTER_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+// C风格函数声明可以放在这里
+// 例如: void some_c_function(float* data, int size);
+
+#ifdef __cplusplus
+} // extern "C"
+#endif
+
+#ifdef __cplusplus
+#include <array>
+#include <cstring>
+#include <stdint.h>
+
+constexpr u8 HISTORY_SIZE = 10; // 历史数据大小
+
+// 环形缓冲区类，用于存储历史数据
+class CircularBuffer {
+  private:
+	std::array<float, HISTORY_SIZE> buffer;
+	size_t head = 0;
+	size_t count = 0;
+
+  public:
+	void enqueue(float value);
+	float get(size_t index) const;
+	void getLatestFive(float *latest_values) const;
+};
+
+float medium_filter(float values[], u8 size);
+
+/*!
+	\brief 电流卡尔曼滤波器类
+	\note  基于电机模型的卡尔曼滤波器实现，用于电流状态估计
+	\note
+	// 创建卡尔曼滤波器实例
+	CurrentKalman kalman(0.0f, 0.0026f, 4.8f, 0.0001f, 1e-6f, 0.02f);
+	// 更新滤波器状态并获取估计值
+	float estimatedCurrent = kalman.update(measuredCurrent, appliedVoltage);
+*/
+class CurrentKalman {
+  private:
+	float x;   // 电流状态估计
+	float P;   // 估计误差协方差
+	float Q;   // 过程噪声协方差
+	float R;   // 测量噪声协方差
+	float L;   // 电机电感(H)
+	float R_m; // 电机电阻(Ω)
+	float T;   // 控制周期(s)
+
+  public:
+	CurrentKalman(float initial_current, float L, float R, float T, float Q,
+				  float R_noise);
+
+	float update(float measured_current, float applied_voltage);
+
+	float getCurrent() const;
+
+	void setProcessNoise(float q);
+
+	void setMeasurementNoise(float r);
+};
+
+	extern CurrentKalman kalman;
+	extern CircularBuffer buffer;
+
+#endif // __cplusplus
+
+#endif // _FILTER_H