RoboDuoduo/r1upper
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

加了个滤波

Browse Source
This commit is contained in:
ws 2025-05-07 22:13:20 +08:00
parent c362ffe265
commit 602a6a2728
6 changed files with 418 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

184
User/lib/filter.c Normal file
View File

@ -0,0 +1,184 @@
/*
各类滤波器。
*/
#include "filter.h"
#include "user_math.h"
/**
* @brief 初始化滤波器
*
* @param f 滤波器
* @param sample_freq 采样频率
* @param cutoff_freq 截止频率
*/
void LowPassFilter2p_Init(LowPassFilter2p_t *f, float sample_freq,
float cutoff_freq) {
if (f == NULL) return;
f->cutoff_freq = cutoff_freq;
f->delay_element_1 = 0.0f;
f->delay_element_2 = 0.0f;
if (f->cutoff_freq <= 0.0f) {
/* no filtering */
f->b0 = 1.0f;
f->b1 = 0.0f;
f->b2 = 0.0f;
f->a1 = 0.0f;
f->a2 = 0.0f;
return;
}
const float fr = sample_freq / f->cutoff_freq;
const float ohm = tanf(M_PI / fr);
const float c = 1.0f + 2.0f * cosf(M_PI / 4.0f) * ohm + ohm * ohm;
f->b0 = ohm * ohm / c;
f->b1 = 2.0f * f->b0;
f->b2 = f->b0;
f->a1 = 2.0f * (ohm * ohm - 1.0f) / c;
f->a2 = (1.0f - 2.0f * cosf(M_PI / 4.0f) * ohm + ohm * ohm) / c;
}
/**
* @brief 施加一次滤波计算
*
* @param f 滤波器
* @param sample 采样的值
* @return float 滤波后的值
*/
float LowPassFilter2p_Apply(LowPassFilter2p_t *f, float sample) {
if (f == NULL) return 0.0f;
/* do the filtering */
float delay_element_0 =
sample - f->delay_element_1 * f->a1 - f->delay_element_2 * f->a2;
if (isinf(delay_element_0)) {
/* don't allow bad values to propagate via the filter */
delay_element_0 = sample;
}
const float output = delay_element_0 * f->b0 + f->delay_element_1 * f->b1 +
f->delay_element_2 * f->b2;
f->delay_element_2 = f->delay_element_1;
f->delay_element_1 = delay_element_0;
/* return the value. Should be no need to check limits */
return output;
}
/**
* @brief 重置滤波器
*
* @param f 滤波器
* @param sample 采样的值
* @return float 滤波后的值
*/
float LowPassFilter2p_Reset(LowPassFilter2p_t *f, float sample) {
if (f == NULL) return 0.0f;
const float dval = sample / (f->b0 + f->b1 + f->b2);
if (isfinite(dval)) {
f->delay_element_1 = dval;
f->delay_element_2 = dval;
} else {
f->delay_element_1 = sample;
f->delay_element_2 = sample;
}
return LowPassFilter2p_Apply(f, sample);
}
/**
* @brief 初始化滤波器
*
* @param f 滤波器
* @param sample_freq 采样频率
* @param notch_freq 中心频率
* @param bandwidth 带宽
*/
void NotchFilter_Init(NotchFilter_t *f, float sample_freq, float notch_freq,float bandwidth)
{
if (f == NULL) return;
f->notch_freq = notch_freq;
f->bandwidth = bandwidth;
f->delay_element_1 = 0.0f;
f->delay_element_2 = 0.0f;
if (notch_freq <= 0.0f) {
/* no filtering */
f->b0 = 1.0f;
f->b1 = 0.0f;
f->b2 = 0.0f;
f->a1 = 0.0f;
f->a2 = 0.0f;
return;
}
const float alpha = tanf(M_PI * bandwidth / sample_freq);
const float beta = -cosf(M_2PI * notch_freq / sample_freq);
const float a0_inv = 1.0f / (alpha + 1.0f);
f->b0 = a0_inv;
f->b1 = 2.0f * beta * a0_inv;
f->b2 = a0_inv;
f->a1 = f->b1;
f->a2 = (1.0f - alpha) * a0_inv;
}
/**
* @brief 施加一次滤波计算
*
* @param f 滤波器
* @param sample 采样的值
* @return float 滤波后的值
*/
inline float NotchFilter_Apply(NotchFilter_t *f, float sample) {
if (f == NULL) return 0.0f;
/* Direct Form II implementation */
const float delay_element_0 = sample - f->delay_element_1 * f->a1 - f->delay_element_2 * f->a2;
const float output = delay_element_0 * f->b0 + f->delay_element_1 * f->b1 +
f->delay_element_2 * f->b2;
f->delay_element_2 = f->delay_element_1;
f->delay_element_1 = delay_element_0;
return output;
}
/**
* @brief 重置滤波器
*
* @param f 滤波器
* @param sample 采样的值
* @return float 滤波后的值
*/
float NotchFilter_Reset(NotchFilter_t *f, float sample) {
if (f == NULL) return 0.0f;
float dval = sample;
if (fabsf(f->b0 + f->b1 + f->b2) > FLT_EPSILON) {
dval = dval / (f->b0 + f->b1 + f->b2);
}
f->delay_element_1 = dval;
f->delay_element_2 = dval;
return NotchFilter_Apply(f, sample);
}

104
User/lib/filter.h Normal file
View File

@ -0,0 +1,104 @@
/*
各类滤波器。
*/
#pragma once
#ifdef __cplusplus
extern "C" {
#endif
#include "user_math.h"
/* 二阶低通滤波器 */
typedef struct {
float cutoff_freq; /* 截止频率 */
float a1;
float a2;
float b0;
float b1;
float b2;
float delay_element_1;
float delay_element_2;
} LowPassFilter2p_t;
/* 带阻滤波器 */
typedef struct {
float notch_freq; /* 阻止频率 */
float bandwidth; /* 带宽 */
float a1;
float a2;
float b0;
float b1;
float b2;
float delay_element_1;
float delay_element_2;
} NotchFilter_t;
/**
* @brief 初始化滤波器
*
* @param f 滤波器
* @param sample_freq 采样频率
* @param cutoff_freq 截止频率
*/
void LowPassFilter2p_Init(LowPassFilter2p_t *f, float sample_freq,
float cutoff_freq);
/**
* @brief 施加一次滤波计算
*
* @param f 滤波器
* @param sample 采样的值
* @return float 滤波后的值
*/
float LowPassFilter2p_Apply(LowPassFilter2p_t *f, float sample);
/**
* @brief 重置滤波器
*
* @param f 滤波器
* @param sample 采样的值
* @return float 滤波后的值
*/
float LowPassFilter2p_Reset(LowPassFilter2p_t *f, float sample);
/**
* @brief 初始化滤波器
*
* @param f 滤波器
* @param sample_freq 采样频率
* @param notch_freq 中心频率
* @param bandwidth 带宽
*/
void NotchFilter_Init(NotchFilter_t *f, float sample_freq, float notch_freq,
float bandwidth);
/**
* @brief 施加一次滤波计算
*
* @param f 滤波器
* @param sample 采样的值
* @return float 滤波后的值
*/
float NotchFilter_Apply(NotchFilter_t *f, float sample);
/**
* @brief 重置滤波器
*
* @param f 滤波器
* @param sample 采样的值
* @return float 滤波后的值
*/
float NotchFilter_Reset(NotchFilter_t *f, float sample);
#ifdef __cplusplus
}
#endif

65
User/lib/kalman.c Normal file
View File

@ -0,0 +1,65 @@
/**
* @author Liu heng
* 一阶卡尔曼滤波器来自RoboMaster论坛
* 一维卡尔曼滤波器
* 使用时先定义一个kalman指针，然后调用kalmanCreate()创建一个滤波器
* 每次读取到传感器数据后即可调用KalmanFilter()来对数据进行滤波
* 使用示例
* extKalman p; //定义一个卡尔曼滤波器结构体
* float SersorData; //需要进行滤波的数据
* KalmanCreate(&p,20,200); //初始化该滤波器的Q=20 R=200参数
* while(1)
* {
* SersorData = sersor(); //获取数据
* SersorData = KalmanFilter(&p,SersorData); //对数据进行滤波
* }
*/
#include "kalman.h"
/**
* @name kalmanCreate
* @brief 创建一个卡尔曼滤波器
* @param p: 滤波器
* T_Q:系统噪声协方差
* T_R:测量噪声协方差
*
* @retval none
* @attention R固定，Q越大，代表越信任侧量值，Q无穷代表只用测量值
* 反之，Q越小代表越信任模型预测值，Q为零则是只用模型预测
*/
void KalmanCreate(extKalman_t *p,float T_Q,float T_R)
{
p->X_last = (float)0;
p->P_last = 0;
p->Q = T_Q;
p->R = T_R;
p->A = 1;
p->B = 0;
p->H = 1;
p->X_mid = p->X_last;
}
/**
* @name KalmanFilter
* @brief 卡尔曼滤波器
* @param p: 滤波器
* dat:待滤波数据
* @retval 滤波后的数据
* @attention Z(k)是系统输入,即测量值 X(k|k)是卡尔曼滤波后的值,即最终输出
* A=1 B=0 H=1 I=1 W(K) V(k)是高斯白噪声,叠加在测量值上了,可以不用管
* 以下是卡尔曼的5个核心公式
* 一阶H'即为它本身,否则为转置矩阵
*/
float KalmanFilter(extKalman_t* p,float dat)
{
p->X_mid =p->A*p->X_last; //百度对应公式(1) x(k|k-1) = A*X(k-1|k-1)+B*U(k)+W(K)
p->P_mid = p->A*p->P_last+p->Q; //百度对应公式(2) p(k|k-1) = A*p(k-1|k-1)*A'+Q
p->kg = p->P_mid/(p->P_mid+p->R); //百度对应公式(4) kg(k) = p(k|k-1)*H'/(H*p(k|k-1)*H'+R)
p->X_now = p->X_mid+p->kg*(dat-p->X_mid); //百度对应公式(3) x(k|k) = X(k|k-1)+kg(k)*(Z(k)-H*X(k|k-1))
p->P_now = (1-p->kg)*p->P_mid; //百度对应公式(5) p(k|k) = (I-kg(k)*H)*P(k|k-1)
p->P_last = p->P_now; //状态更新
p->X_last = p->X_now;
return p->X_now; //输出预测结果x(k|k)
}

28
User/lib/kalman.h Normal file
View File

@ -0,0 +1,28 @@
/**
* @author Liu heng
* 卡尔曼滤波器来自RoboMaster论坛
*/
#ifndef _KALMAN_H
#define _KALMAN_H
typedef struct {
float X_last; //上一时刻的最优结果 X(k-|k-1)
float X_mid; //当前时刻的预测结果 X(k|k-1)
float X_now; //当前时刻的最优结果 X(k|k)
float P_mid; //当前时刻预测结果的协方差 P(k|k-1)
float P_now; //当前时刻最优结果的协方差 P(k|k)
float P_last; //上一时刻最优结果的协方差 P(k-1|k-1)
float kg; //kalman增益
float A; //系统参数
float B;
float Q;
float R;
float H;
}extKalman_t;
void KalmanCreate(extKalman_t *p,float T_Q,float T_R);
float KalmanFilter(extKalman_t* p,float dat);
#endif

29
User/module/ball.cpp
View File

@ -32,8 +32,8 @@ Ball ::Ball()
PID_init(&extend_speed_pid,PID_POSITION,Extend_speed_PID,3000, 200);
PID_init(&extend_angle_pid,PID_POSITION,Extend_angle_PID,3000, 200);
// e_result = 0;
// angleSet = 0;
e_result = 0;
angleSet = 0;
//三摩擦轮
for(int i = 0;i < MOTOR_NUM;i ++)
{
@ -47,9 +47,22 @@ Ball ::Ball()
speedSet[i] = 0;
}
//状态机状态初始化
currentState1= BALL_IDLE;
}
void Ball :: Filter_init(float target_freq)
{
for(int i = 0;i < MOTOR_NUM;i ++)
{
//摩擦轮滤波器初始化
LowPassFilter2p_Init(filter.in + i , target_freq,
-1.0f);
LowPassFilter2p_Init(filter.out + i, target_freq,
-1.0f);
}
}
void Ball ::Extend_control(int angle)
@ -72,6 +85,11 @@ void Ball ::Extend_control(int angle)
void Ball ::Spin(float speed,float speed2)
{
for(int i = 0;i < MOTOR_NUM;i ++)
{
hand_Motor[i]->speed_rpm=LowPassFilter2p_Apply(filter.in + i, hand_Motor[i]->speed_rpm);
}
speedSet[MOTOR_1] = -speed;
result[MOTOR_1] = PID_calc(&speed_pid[MOTOR_1],hand_Motor[MOTOR_1]->speed_rpm,speedSet[MOTOR_1]);
@ -81,6 +99,11 @@ void Ball ::Spin(float speed,float speed2)
speedSet[MOTOR_3] = speed2;
result[MOTOR_3] = PID_calc(&speed_pid[MOTOR_3],hand_Motor[MOTOR_3]->speed_rpm,speedSet[MOTOR_3]);
for(int i = 0;i < MOTOR_NUM;i ++)
{
result[i]=LowPassFilter2p_Apply(filter.out + i, result[i]);
}
}
int flag =0;

11
User/module/ball.hpp
View File

@ -8,6 +8,7 @@
#include "bsp_delay.h"
#include "djiMotor.h"
#include "pid.h"
#include "filter.h"
// 定义状态枚举
typedef enum {
@ -33,6 +34,12 @@ typedef enum
MOTOR_NUM
}motorhangd_e;
/* 滤波器 */
typedef struct {
LowPassFilter2p_t *in; /* 反馈值滤波器 */
LowPassFilter2p_t *out; /* 输出值滤波器 */
} Filter;
// 定义光电传感器检测宏
#define IS_PHOTOELECTRIC_BALL() (HAL_GPIO_ReadPin(up_ball_GPIO_Port, up_ball_Pin) == GPIO_PIN_RESET)
@ -40,6 +47,7 @@ class Ball
{
public:
Ball();
void Filter_init(float target_freq);
void Spin(float speed,float speed2);
void ballHadling(void);
void Send_control(void);
@ -59,6 +67,9 @@ public:
volatile uint32_t flag_thread;//接收传回的线程通知
private:
//滤波一下
Filter filter;
//三个摩擦轮
static const float M3508_speed_PID[3];
static const float M3508_angle_PID[3];