常见信号滤波方法（卡尔曼滤波、滑动平均、异常值剔除）的原理解析与C语言实现

最新推荐文章于 2024-07-10 17:36:54 发布
最新推荐文章于 2024-07-10 17:36:54 发布
阅读量5.7k 收藏 86
点赞数 5
分类专栏： STM32-MCU 文章标签： c语言 信号滤波 卡尔曼滤波 滑动平均滤波 异常值剔除滤波
版权声明：本文为博主原创文章，遵循 CC 4.0 BY-SA 版权协议，转载请附上原文出处链接和本声明。
本文链接：https://blog.csdn.net/u013441358/article/details/132675400
版权

常见信号滤波方法（卡尔曼滤波、滑动平均、异常值剔除）的原理解析与C语言实现

日期作者版本备注
2023.09.04Dog TaoV1.0完成文档的初始版本。

文章目录

前言

卡尔曼滤波、滑动平均和异常值剔除是信号处理和数据分析中常用的滤波和平滑技术。这些方法旨在从测量或采集的数据中提取有价值的信息，同时减少噪声和不必要的波动。

卡尔曼滤波起源于1960年，由Rudolf E. Kalman开发，用于解决航天任务中的导航和控制问题。卡尔曼滤波为线性动态系统提供了一个递归的解决方案，用于预测和估计系统的状态。在存在噪声的情况下，它可以提供对系统状态的最优估计。它广泛应用于导航、经济学、机器人技术和许多其他领域。

滑动平均是时间序列分析中的一种基本技术，存在已久，被广泛应用于各种领域，从经济学到工程学。通过计算数据点的局部集合的平均值来平滑数据，从而降低随机波动和噪声。滑动平均简单、直观且计算高效，适用于实时系统和任何需要平滑数据的应用，从股市预测到传感器数据处理。

在数据分析中，数据往往包含错误、噪声或异常值，这些值可能是由测量错误、设备故障或其他非标准事件引起的。通过识别和删除（或替换）这些异常值，可以减少数据的误差，提高分析的准确性。异常值剔除滤波保证了数据的质量和可靠性，为后续的分析、预测和决策提供了更稳健的基础。

本文档使用C语言（运行平台Cortex-M3/M4内核单片机）针对最简单的一维线性信号（例如温度、压力、流量等数据采集）实现了上述三种信号处理方法。为了最大程度上实现示例与教学的效果，三种算法的实现分别基于构造类型结构体、定义缓存数组、动态内存分配等方式，可供读者对比分析或加以修改利用。

卡尔曼滤波

算法简述

卡尔曼滤波 (Kalman Filter)是一个递归的算法，用于从一系列的测量中估计过程的状态，即使测量中含有噪声。它最初被用于航空和宇宙工程，但现在在各种领域都有应用，包括经济学、控制理论和机器人技术。它旨在优化给定的不完美测量和估计状态之间的权重。卡尔曼滤波有很多变种，如扩展卡尔曼滤波、无迹卡尔曼滤波等，适用于非线性和非高斯问题。

卡尔曼滤波是一个迭代过程，用于估计线性动态系统的状态，当存在测量噪声和过程噪声时。它由两个主要步骤组成：预测（或时间更新）和更新（或测量更新）。假设我们有一个系统状态 x x x和测量 z z z，卡尔曼滤波的基本思想是结合预测的状态和通过测量获得的新信息来估计系统的真实状态。

以下是卡尔曼滤波的基本步骤：

  1. 初始化:

    • 估计的初始状态： x ^ 0 \hat{x}_0 x^0​
    • 估计的初始状态的协方差： P 0 P_0 P0​

  2. 预测 (时间更新):

    • 状态预测:
      x ^ k ∣ k − 1 = A x ^ k − 1 ∣ k − 1 + B u k \hat{x}_{k|k-1} = A \hat{x}_{k-1|k-1} + B u_k x^k∣k−1​=Ax^k−1∣k−1​+Buk​
    • 协方差预测:
      P k ∣ k − 1 = A P k − 1 ∣ k − 1 A T + Q P_{k|k-1} = A P_{k-1|k-1} A^T + Q Pk∣k−1​=APk−1∣k−1​AT+Q
      其中:
      A A A 是状态转移矩阵，
      B B B 是控制输入矩阵，
      u k u_k uk​ 是控制输入，
      Q Q Q 是过程噪声的协方差矩阵。

  3. 更新 (测量更新):

    • 计算卡尔曼增益:
      K k = P k ∣ k − 1 H T ( H P k ∣ k − 1 H T + R ) − 1 K_k = P_{k|k-1} H^T (H P_{k|k-1} H^T + R)^{-1} Kk​=Pk∣k−1​HT(HPk∣k−1​HT+R)−1
    • 状态更新:
      x ^ k ∣ k = x ^ k ∣ k − 1 + K k ( z k − H x ^ k ∣ k − 1 ) \hat{x}_{k|k} = \hat{x}_{k|k-1} + K_k (z_k - H \hat{x}_{k|k-1}) x^k∣k​=x^k∣k−1​+Kk​(zk​−Hx^k∣k−1​)
    • 协方差更新:
      P k ∣ k = ( I − K k H ) P k ∣ k − 1 P_{k|k} = (I - K_k H) P_{k|k-1} Pk∣k​=(I−Kk​H)Pk∣k−1​
      其中:
      H H H 是观测矩阵，
      R R R 是测量噪声的协方差矩阵。

其中，预测步骤负责推进状态和协方差到下一时刻。更新步骤则是当新的测量数据可用时，利用这些数据来校正预测，提供了一个更新的估计。

注: 这是线性卡尔曼滤波的简化描述，适用于线性系统和高斯噪声。对于非线性系统或非高斯噪声，需要使用扩展卡尔曼滤波 (EKF)、无迹卡尔曼滤波 (UKF) 等变种。

参考源码

  • 头文件
#ifndef __FILTER_H__
#define __FILTER_H__

#include "platform.h"

#include "FreeRTOS.h"
#include "portable.h"


/// @brief 定义滤波算法库中使用的动态内存分配方法
#define FILTER_MALLOC  pvPortMalloc     // 使用FreeRTOS实时系统的内存分配方法
// #define FILTER_MALLOC  malloc        // 使用标准C库的内存分配方法

/**
 * @brief 卡尔曼滤波结构体
 *      为每个卡尔曼滤波的通道都定义一个卡尔曼滤波结构体变量
 */
typedef struct
{
    /// @brief Q代表预测值的方差，R代表测量值的方差，更小方差对应的量对下次的预测值有更大的影响权重
    float Q_ProcessNoise;                      // 预测值的方差, Q_ProcessNoise:过程噪声，Q增大，动态响应变快，收敛稳定性变坏
    float R_MeasureNoise;                      // 测量值的方差, R_MeasureNoise:测量噪声，R增大，动态响应变慢，收敛稳定性变好

    float estimate_value_init;    // 初始时刻温度的最优估计值
    float estimate_variance_init; // 初始时刻温度的估计方差

    float estimate_value_curr;    // 当前温度的最优估计值
    float estimate_variance_curr; // 当前温度的估计方差

    float predict_value_next;    // 下一个状态的预测值
    float predict_variance_next; // 下一个状态的估计方差
} Kalman_Filter_Struct;

/**
 * @brief 卡尔曼滤波初始化，主要是实现卡尔曼滤波结构体变量的成员初始化
 *      针对温度采集信号等类似问题设计，即初始状态选取为初始测量值、当前时刻的温度估计值为上一时刻的温度。
 * @param kalman_filter 卡尔曼滤波结构体变量
 * @param Q_ProcessNoise Q值，过程噪声，预测值的方差
 * @param R_MeasureNoise R值，测量噪声，测量值的方差
 * @param init_estimate_value 初始时刻温度的估计值
 * @param init_estimate_variance 初始时刻温度的估计方差
 */
void Kalman_Filter_Init(Kalman_Filter_Struct *kalman_filter, float Q_ProcessNoise, float R_MeasureNoise, float init_estimate_value, float init_estimate_variance);

/**
 * @brief 卡尔曼滤波迭代
 *      滤波器估计过程中某一个时刻的状态，利用量测更新的值作为反馈。所以卡尔曼滤波过程又可以分为状态跟新和测量更新两个部分来实现。
 *      状态更新中主要是为了获得下个时刻的先验估计，量测更新则是为了通过先验估计和量测值获取后验估计。
 * @param kalman_filter 卡尔曼滤波结构体变量
 * @param measure_value_curr 当前量测值
 * @return float 卡尔曼滤波值
 */
float Kalman_Filter_Iterate(Kalman_Filter_Struct *kalman_filter, float measure_value_curr);

#endif
  • 源文件
void Kalman_Filter_Init(Kalman_Filter_Struct *kalman_filter, float Q_ProcessNoise, float R_MeasureNoise, float init_estimate_value, float init_estimate_variance)
{
    kalman_filter->Q_ProcessNoise = Q_ProcessNoise;
    kalman_filter->R_MeasureNoise = R_MeasureNoise;

    kalman_filter->estimate_value_init = init_estimate_value;
    kalman_filter->estimate_variance_init = init_estimate_variance;

    kalman_filter->estimate_value_curr = kalman_filter->estimate_value_init;
    kalman_filter->estimate_variance_curr = kalman_filter->estimate_variance_init;
}


float Kalman_Filter_Iterate(Kalman_Filter_Struct *kalman_filter, float measure_value_curr)
{
    // 利用当前估计值迭代下次预测值
    kalman_filter->predict_value_next = kalman_filter->estimate_value_curr;
    kalman_filter->predict_variance_next = kalman_filter->estimate_variance_curr + kalman_filter->Q_ProcessNoise;

    // 计算卡尔曼增益
    float k_index = kalman_filter->predict_variance_next / (kalman_filter->predict_variance_next + kalman_filter->R_MeasureNoise);

    // 利用下次预测值迭代下次估计值
    // 例如在进行温度预测时，因为温度是一个连续的状态，我们认为上一时刻的温度和当前时刻的温度相等，则有T(k)=T(k-1)
    kalman_filter->estimate_value_curr = k_index * measure_value_curr + (1 - k_index) * kalman_filter->predict_value_next;
    kalman_filter->estimate_variance_curr = (1 - k_index) * kalman_filter->predict_variance_next;

    // 输出下次估计值
    return kalman_filter->estimate_value_curr;
}

调用示例

  • 初始化滤波器（通过声明卡尔曼结构体变量的方式）

定义卡尔曼滤波结构体变量

Kalman_Filter_Struct kalman_filter_pt1 =
{
	.Q_ProcessNoise = 0.001,
	.R_MeasureNoise = 0.1,
	.estimate_value_init = 25,
	.estimate_variance_init = 1,
	.estimate_value_curr = 25,
	.estimate_variance_curr = 1,
};

初始化卡尔曼滤波器

	Kalman_Filter_Init(&kalman_filter_pt1, 0.001, 2, 25, 1);
  • 使用滤波器
	float pt1_KMfilter = Kalman_Filter_Iterate(&kalman_filter_pt1, SenPt100Array[0].CalibValue);

滑动平均滤波

算法简述

滑动平均滤波 (Moving Average Filter)是一种简单的滤波方法，其核心思想是用一组最近的数据点的算术平均值来估计当前的值。这种方法的优点是简单且实现起来容易。它能有效地减少随机噪声。但滑动平均滤波对于处理"尖峰值"或突发性的数据变化可能不是非常有效，因为它可能会对数据产生滞后。

下面是滑动平均滤波的基本实现方法：

  1. 选择窗口大小：首先，选择一个窗口大小 N N N，这决定了用于计算平均值的连续数据点的数量。例如，对于一个大小为3的窗口，你会使用当前点及其前两个数据点来计算平均值。

  2. 计算平均值：对于数据中的每个点（从第 N N N个点开始，因为前 N − 1 N-1 N−1个点没有足够的前数据点来计算滑动平均），计算该点及其前 N − 1 N-1 N−1个点的算术平均值。

假设数据序列为 x 1 , x 2 , x 3 , … , x i , … x_1, x_2, x_3, \dots, x_i, \dots x1​,x2​,x3​,…,xi​,…，其对应的滑动平均序列为 m a 1 , m a 2 , m a 3 , … , m a i , … ma_1, ma_2, ma_3, \dots, ma_i, \dots ma1​,ma2​,ma3​,…,mai​,…，对于每个点 x i x_i xi​(其中 i ≥ N i \geq N i≥N)，其滑动平均值计算如下：

m a i = x i − N + 1 + x i − N + 2 + ⋯ + x i N ma_i = \frac{x_{i-N+1} + x_{i-N+2} + \dots + x_i}{N} mai​=Nxi−N+1​+xi−N+2​+⋯+xi​​

  1. 滑动窗口：每次新的数据点进入窗口时，最旧的数据点离开窗口。然后再次计算新窗口中数据的平均值。

注意：

  • 选择不同的窗口大小 N N N会影响结果。较大的 N N N会导致更平滑的数据，但可能引入更大的滞后；较小的 N N N会减少滞后，但结果可能较为嘈杂。
  • 有许多滑动平均滤波的变种，如加权移动平均，其中数据点的权重不同，使得某些点在平均值中的影响更大。
  • 滑动平均滤波非常简单且计算量小，适用于实时系统和资源受限的环境。

简单的Python实现示例：

def moving_average(data, N=3):
    return [sum(data[i-N:i]) / N for i in range(N, len(data)+1)]

参考源码

  • 头文件
#ifndef __FILTER_H__
#define __FILTER_H__

#include "platform.h"

#include "FreeRTOS.h"
#include "portable.h"


/// @brief 定义滤波算法库中使用的动态内存分配方法
#define FILTER_MALLOC  pvPortMalloc     // 使用FreeRTOS实时系统的内存分配方法
// #define FILTER_MALLOC  malloc        // 使用标准C库的内存分配方法

/// 由于滑动平均滤波算法采用了静态数组空间作为工作内存，因此需要提前预设好最大数组通道数与数组容量。
/// 为了保证算法库的封装性，一般在本源码的外部（项目业务代码中）定义下述变量（FILTER_MA_QueueNum, FILTER_MA_QueueSize）,
/// 并将下述指针（MAFilter_DataQueue_P, MAFilter_Flag_IsDataQueueFull_P, MAFilter_Flag_DataCount_P, MAFilter_Sum_P）
/// 赋值为在本源码的外部（项目业务代码中）定义的数组地址。
extern uint8_t const FILTER_MA_QueueNum; // 滑动平均滤波 最大通道数
extern uint16_t const FILTER_MA_QueueSize; // 滑动平均滤波 最大队列深度

extern int32_t *const MAFilter_DataQueue_P; // 滑动平均滤波 数据队列二维数组 e.g. MAFilter_DataQueue[FILTER_MA_QUEUENUM][FILTER_MA_QUEUESIZE]
extern uint8_t *const MAFilter_Flag_IsDataQueueFull_P;  // 滑动平均滤波 队列满标志位数组
extern uint8_t *const MAFilter_Flag_DataCount_P;    // 滑动平均滤波 队列当前容量标志数组
extern int64_t *const MAFilter_Sum_P;       // 滑动平均滤波 用于存储int32_t类型的队列数据累加


/**
 * @brief 滑动平均滤波功能初始化，将数据缓存清空置零
 * 
 */
void Filter_MovingAverage_Init(void);

/**
 * @brief 按照固定的队列深度进行滑动平均滤波（输入原始数据，输出滤波后的数据）
 * @param _data 输入的数据
 * @param channel 输入的通道
 * @return int32_t 输出滤波后的数据
 */
int32_t Filter_MovingAverage(int32_t _data, uint8_t channel);

/**
 * @brief 按照可变的队列深度进行滑动平均滤波（输入原始数据，输出滤波后的数据）
 * @param _data 输入的数据
 * @param channel 输入的通道
 * @param size 滤波队列的深度
 * @return int32_t 输出滤波后的数据
 */
int32_t Filter_MovingAverage_VariantSize(int32_t _data, uint8_t channel, uint8_t size);

#endif
  • 源文件
void Filter_MovingAverage_Init()
{
	uint8_t i, j;
	for (i = 0; i < FILTER_MA_QueueNum; i++)
	{
		MAFilter_Flag_IsDataQueueFull_P[i] = 0;
		MAFilter_Flag_DataCount_P[i] = 0;

		for (j = 0; j < FILTER_MA_QueueSize; j++)
		{
//			MAFilter_DataQueue_P[i][j] = 0;
		    MAFilter_DataQueue_P[i*FILTER_MA_QueueSize+j] = 0;
		}
	}
}

int32_t Filter_MovingAverage(int32_t _data, uint8_t channel)
{
	int16_t sumNum;

	MAFilter_Sum_P[channel] = 0;

//	MAFilter_DataQueue_P[channel][MAFilter_Flag_DataCount_P[channel]] = _data;
	MAFilter_DataQueue_P[channel*FILTER_MA_QueueSize+MAFilter_Flag_DataCount_P[channel]] = _data;

	MAFilter_Flag_DataCount_P[channel]++;

	if (MAFilter_Flag_DataCount_P[channel] == FILTER_MA_QueueSize)
	{
		MAFilter_Flag_DataCount_P[channel] = 0;
		MAFilter_Flag_IsDataQueueFull_P[channel] = 1;
	}

	if (!MAFilter_Flag_IsDataQueueFull_P[channel])
	{
		sumNum = MAFilter_Flag_DataCount_P[channel];
	}
	else
	{
		sumNum = FILTER_MA_QueueSize;
	}

	for (uint8_t i = 0; i < sumNum; i++)
	{
//		MAFilter_Sum_P[channel] += MAFilter_DataQueue_P[channel][i];
		MAFilter_Sum_P[channel] += MAFilter_DataQueue_P[channel*FILTER_MA_QueueSize+i];
	}
	return (int32_t) (MAFilter_Sum_P[channel] / sumNum);
}

int32_t Filter_MovingAverage_VariantSize(int32_t _data, uint8_t channel, uint8_t size)
{
	int16_t sumNum;

	MAFilter_Sum_P[channel] = 0;

//	MAFilter_DataQueue_P[channel][MAFilter_Flag_DataCount_P[channel]] = _data;
	MAFilter_DataQueue_P[channel*FILTER_MA_QueueSize+MAFilter_Flag_DataCount_P[channel]] = _data;

	MAFilter_Flag_DataCount_P[channel]++;

	if (MAFilter_Flag_DataCount_P[channel] == FILTER_MA_QueueSize)
	{
		MAFilter_Flag_DataCount_P[channel] = 0;
		MAFilter_Flag_IsDataQueueFull_P[channel] = 1;
	}

	if (!MAFilter_Flag_IsDataQueueFull_P[channel])
	{
		sumNum = MAFilter_Flag_DataCount_P[channel];
	}
	else
	{
		sumNum = FILTER_MA_QueueSize;
	}
/Variant Size/
	//data queue is not full and data num is less than variant size
	//sumNum <= size
	if (size >= sumNum)
	{
		for (uint8_t i = 0; i < sumNum; i++)
		{
//			Filter_Sum_P[channel] += MAFilter_DataQueue_P[channel][i];
		    MAFilter_Sum_P[channel] += MAFilter_DataQueue_P[channel*FILTER_MA_QueueSize+i];
		}
	}
	//sumNum > size
	else
	{
		//data queue is not full
		//size < sumNum < QueueSize
		if (!MAFilter_Flag_IsDataQueueFull_P[channel])
		{
			for (uint8_t i = sumNum - size; i < sumNum; i++)
			{
//				MAFilter_Sum_P[channel] += MAFilter_DataQueue_P[channel][i];
			    MAFilter_Sum_P[channel] += MAFilter_DataQueue_P[channel*FILTER_MA_QueueSize+i];
			}
		}
		//data queue is full
		//sumNum = QueueSize
		else
		{
			if (MAFilter_Flag_DataCount_P[channel] >= size)
			{
				for (uint8_t i = MAFilter_Flag_DataCount_P[channel] - size; i < MAFilter_Flag_DataCount_P[channel]; i++)
				{
//					MAFilter_Sum_P[channel] += MAFilter_DataQueue_P[channel][i];
					MAFilter_Sum_P[channel] += MAFilter_DataQueue_P[channel*FILTER_MA_QueueSize+i];
				}
			}
			else
			{
				for (uint8_t i = sumNum - (size - MAFilter_Flag_DataCount_P[channel]); i < FILTER_MA_QueueSize; i++)
				{
//				    MAFilter_Sum_P[channel] += MAFilter_DataQueue_P[channel][i];
					MAFilter_Sum_P[channel] += MAFilter_DataQueue_P[channel*FILTER_MA_QueueSize+i];
				}
				for (uint8_t i = 0; i < MAFilter_Flag_DataCount_P[channel]; i++)
				{
//				    MAFilter_Sum_P[channel] += MAFilter_DataQueue_P[channel][i];
					MAFilter_Sum_P[channel] += MAFilter_DataQueue_P[channel*FILTER_MA_QueueSize+i];
				}
			}
		}
		sumNum = size;
	}
//
	return (int32_t) (MAFilter_Sum_P[channel] / sumNum);
}

调用示例

  • 初始化滤波器（通过定义缓存数组的方式）

滤波器用户文件-头文件

#ifndef _FILTER_USER_H_
#define _FILTER_USER_H_

#include "filter.h"

#define FILTER_MA_QUEUENUM 				4
#define FILTER_MA_QUEUESIZE 			50

#endif

滤波器用户文件-源文件

#include "filter_user.h"

uint8_t const FILTER_MA_QueueNum = FILTER_MA_QUEUENUM;
uint16_t const FILTER_MA_QueueSize = FILTER_MA_QUEUESIZE;

static int32_t MAFilter_DataQueue[FILTER_MA_QUEUENUM][FILTER_MA_QUEUESIZE] = { 0 };
static uint8_t MAFilter_Flag_IsDataQueueFull[FILTER_MA_QUEUENUM] = { 0 };
static uint8_t MAFilter_Flag_DataCount[FILTER_MA_QUEUENUM] = { 0 };
static int32_t MAFilter_Sum[FILTER_MA_QUEUENUM] = { 0 };

int32_t *const MAFilter_DataQueue_P = (int32_t *)MAFilter_DataQueue;
uint8_t *const MAFilter_Flag_IsDataQueueFull_P = MAFilter_Flag_IsDataQueueFull;
uint8_t *const MAFilter_Flag_DataCount_P = MAFilter_Flag_DataCount;
int32_t *const MAFilter_Sum_P = MAFilter_Sum;

初始化滤波器

    Filter_MovingAverage_Init();
  • 使用滤波器
float Temp1_filter = Filter_MovingAverage(Temp1 * 10.0, 2) / 10.0;     
float BatVol_filter = Filter_MovingAverage_VariantSize(BatVol * 10.0, 3, 50) / 10.0;

异常值剔除滤波

算法简述

异常值剔除滤波（Outlier Reject Filter）是一种基于统计方法的滤波技术。其核心思想是先识别出数据中的异常值或噪声，然后将其剔除或替换。通常，这种方法涉及到设定一个阈值。任何超过这个阈值的数据都被视为异常值。异常值剔除滤波的一种常见的实现方法是剔除数据集中的最大和最小值。当结合滑动平均时，这种方法可以进一步平滑数据，同时减少由于极端值导致的不必要的波动。

以下是该方法的详细实现步骤：

  1. 选择窗口大小：首先，确定一个窗口大小 N N N。这决定了用于计算平均值的连续数据点的数量。

  2. 选择剔除数量：确定要从每个窗口中剔除的最大值和最小值的数量，称之为 M M M。这意味着，从每个窗口中，你将剔除 M M M个最大值和 M M M个最小值。

  3. 滑动窗口 & 剔除异常值：对于数据中的每个窗口（从第 N N N个点开始），做以下操作：

    • 从窗口中找出 M M M个最大值和 M M M个最小值。
    • 从窗口中剔除这些极端值。
    • 计算窗口中剩余数据点的平均值。

  4. 滑动窗口：每次新的数据点进入窗口时，最旧的数据点离开窗口。然后再次执行上述步骤。

简单的Python实现示例：

def outlier_removal_moving_average(data, N=5, M=1):
    # 初始化一个空列表存放滤波后的结果
    filtered_data = []
    
    for i in range(N-1, len(data)):
        window = data[i-N+1:i+1]  # 获取窗口数据
        
        # 剔除M个最大值和M个最小值
        for _ in range(M):
            window.remove(max(window))
            window.remove(min(window))
        
        # 计算平均值并加入到滤波后的数据列表中
        filtered_data.append(sum(window) / len(window))
    
    return filtered_data

此函数使用窗口大小 (N) 和剔除数量 (M) 来对数据进行滤波。这是一个简单的实现，可能需要根据实际数据和应用进行一些调整和优化。

参考源码

  • 头文件
#ifndef __FILTER_H__
#define __FILTER_H__

#include "platform.h"

#include "FreeRTOS.h"
#include "portable.h"


/// @brief 定义滤波算法库中使用的动态内存分配方法
#define FILTER_MALLOC  pvPortMalloc     // 使用FreeRTOS实时系统的内存分配方法
// #define FILTER_MALLOC  malloc        // 使用标准C库的内存分配方法


/**
 * @brief 异常剔除滤波功能初始化，分配数据缓存空间
 * @param channel_size 输入数据队列深度
 * @param channel_num 输入通道数量
 * @return int 内存分配结果
 *  @arg 0 内存分配成功
 *  @arg -1 内存分配失败
 */
int Filter_OutlierReject_Init(uint16_t channel_size, uint16_t channel_num);

/**
 * @brief 异常剔除滤波（输入原始数据，输出滤波后的数据）
 *      构造FIFO数据队列，往队列中输入数据。将队列中的数据排序，取中间部分的数据计算均值输出。
 *      如此可以剔除队列中最大、最小的一个或几个异常数据，并将剩余数据进行了滑动平均处理。
 * @param data 输入的数据
 * @param channel 输入的通道
 * @param avg_size 计算均值的数据长度
 * @return int32_t 输出滤波后的数据
 */
int32_t Filter_OutlierReject(int32_t _data, uint16_t channel, uint16_t avg_size);

#endif
  • 源文件

static int32_t *Filter_OutlierReject_Buffer = NULL;		// 异常值剔除滤波 数据缓存指针
static int32_t *Filter_OutlierReject_SortBuffer = NULL; // 异常值剔除滤波 数据排序缓存指针
static uint8_t *Filter_OutlierReject_IsDataQueueFull = NULL; // 异常值剔除滤波 队列满标志位指针
static uint8_t *Filter_OutlierReject_DataCount = NULL;	// 异常值剔除滤波 队列数据量标志位指针
static int16_t Filter_OutlierReject_BufferSize = 0;		// 异常值剔除滤波 队列深度
static int16_t Filter_OutlierReject_BufferChannel = 0;	// 异常值剔除滤波 队列数

int Filter_OutlierReject_Init(uint16_t channel_size, uint16_t channel_num)
{
	Filter_OutlierReject_Buffer = (int32_t *)FILTER_MALLOC(sizeof(int32_t)*channel_size*channel_num);
	Filter_OutlierReject_SortBuffer = (int32_t *)FILTER_MALLOC(sizeof(int32_t)*channel_size);
	Filter_OutlierReject_IsDataQueueFull = (uint8_t *)FILTER_MALLOC(sizeof(uint8_t)*channel_num);
	Filter_OutlierReject_DataCount = (uint8_t *)FILTER_MALLOC(sizeof(uint8_t)*channel_num);

	Filter_OutlierReject_BufferSize = channel_size;
	Filter_OutlierReject_BufferChannel = channel_num;

	// 内存是否分配成功
	if(Filter_OutlierReject_Buffer == NULL || Filter_OutlierReject_IsDataQueueFull == NULL || 
	Filter_OutlierReject_DataCount == NULL || Filter_OutlierReject_SortBuffer == NULL)
	{
		return -1;
	}

	// 初始化为0
	memset(Filter_OutlierReject_Buffer, 0, sizeof(int32_t)*channel_size*channel_num);
	memset(Filter_OutlierReject_SortBuffer, 0, sizeof(int32_t)*channel_size);
	memset(Filter_OutlierReject_IsDataQueueFull, 0, sizeof(uint8_t)*channel_num);
	memset(Filter_OutlierReject_DataCount, 0, sizeof(uint8_t)*channel_num);

	return 0;
}


int32_t Filter_OutlierReject(int32_t _data, uint16_t channel, uint16_t avg_size)
{
	int16_t sumNum = 0;
	int32_t dataTemp_sort = 0;
	int64_t dataTemp_sum = 0;		// 用于存储int32类型的数值累加
	int32_t dataReturn = 0;

	// 内存是否分配成功
	if(Filter_OutlierReject_Buffer == NULL || Filter_OutlierReject_IsDataQueueFull == NULL || 
	Filter_OutlierReject_DataCount == NULL || Filter_OutlierReject_SortBuffer == NULL)
	{
		return _data;
	}

	Filter_OutlierReject_Buffer[channel*Filter_OutlierReject_BufferSize+Filter_OutlierReject_DataCount[channel]] = _data;

	Filter_OutlierReject_DataCount[channel]++;

	if (Filter_OutlierReject_DataCount[channel] == Filter_OutlierReject_BufferSize)
	{
		Filter_OutlierReject_DataCount[channel] = 0;
		Filter_OutlierReject_IsDataQueueFull[channel] = 1;
	}

	if (!Filter_OutlierReject_IsDataQueueFull[channel])
	{
		sumNum = Filter_OutlierReject_DataCount[channel];
	}
	else
	{
		sumNum = Filter_OutlierReject_BufferSize;
	}

	// 将数据拷贝到排序缓存中进行排序，以免影响原FIFO队列
	memcpy(Filter_OutlierReject_SortBuffer, Filter_OutlierReject_Buffer + channel*Filter_OutlierReject_BufferSize, sizeof(int32_t)*sumNum);

	// Sort the data array
	for(uint16_t i = 0; i < sumNum - 1; i++)
	{
		for(uint16_t j = 0; j < sumNum-1-i; j++)
		{
			if(Filter_OutlierReject_SortBuffer[j] > Filter_OutlierReject_SortBuffer[j+1])
			{
				dataTemp_sort = Filter_OutlierReject_SortBuffer[j];
				Filter_OutlierReject_SortBuffer[j] = Filter_OutlierReject_SortBuffer[j+1];
				Filter_OutlierReject_SortBuffer[j+1] = dataTemp_sort;
			}
		}
	}

	// return the median of the data array
	if(avg_size >= sumNum || avg_size <= 1)
	{
		dataReturn = Filter_OutlierReject_SortBuffer[sumNum/2];
	}
	else
	{
		for(uint16_t i = sumNum/2 - avg_size/2; i < sumNum/2 + avg_size/2; i++)
		{
			dataTemp_sum += Filter_OutlierReject_SortBuffer[i];
		}

		dataReturn = dataTemp_sum/((avg_size/2)*2);
	}

	return dataReturn;
}

调用示例

  • 初始化滤波器（通过动态分配内存的方式）
	/**
	 * @brief 异常剔除滤波算法，队列长度为10，队列通道为10
	 * 		10个通道依次为：电阻测量通道*4，温度测量通道*4，温湿度传感器通道*2
	 */
	int rest = Filter_OutlierReject_Init(10, 10);
  • 使用滤波器

下述示例中演示了如何在仅支持int32_t类型数据输入的滤波器中实现浮点数保留指定位数的小数。

	//通道1-电阻值数据
	InputReg_SetData_Float(8, Filter_OutlierReject(SenResMeasArray[0].CalibValue * 1000, 0, 6) / 1000.0);
	//通道2-电阻值数据
	InputReg_SetData_Float(9, Filter_OutlierReject(SenResMeasArray[1].CalibValue * 1000, 1, 6) / 1000.0);
	//通道3-电阻值数据
	InputReg_SetData_Float(10, Filter_OutlierReject(SenResMeasArray[2].CalibValue * 1000, 2, 6) / 1000.0);
	//通道4-电阻值数据
	InputReg_SetData_Float(11, Filter_OutlierReject(SenResMeasArray[3].CalibValue * 1000, 3, 6) / 1000.0);
全能骑士涛锅锅
关注
  • 5
    点赞
  • 踩
  • 86
    收藏
    觉得还不错? 一键收藏
  • 打赏
    打赏
  • 0
    评论
专栏目录
C语言实现简单卡尔曼滤波
sin1111yi的博客
12-11 1万+
C语言实现简单卡尔曼滤波 https://www.bilibili.com/video/BV1ez4y1X7eR DR.CAN讲的真的很好 卡尔曼滤波的步骤 步骤 说明 Step 1 计算卡尔曼增益 Step 2 更新本次迭代的估计值 Step 3 更新本次迭代的估计误差 具体请看上面DR.CAN的视频 代码 参数 说明 x_mea 测量值 x_est 估计值 e_mea 固有的测量误差，取决于测量工具的精度，假设测量工具量程是2000/%2，测量误差就是
C语言实现的滑动平均滤波算法
热门推荐
爱跳舞的大象的博客
09-27 5万+
最近采集压力传感器的电压信号，采集到的数据波动挺大的，所以采用滤波的方法进行处理一下。     因为在matlab中依靠matlab的画图函数很容易看到滤波后的数据图像，进而验证滤波效果。最后选择滤波效果好的滤波方法进行C语言实现。     所以首先将采集到的原始数据保存到TXT文档里面，然后导入到matlab中，在matlab中对数据进行滤波。     在matlab中采用了三种滤波方法：1、滑动平均滤波 2、中值滤波 3、卡尔曼滤
纯c语言实现卡尔曼滤波
11-04
利用纯c语言实现卡尔曼滤波，已通过编译，可以完美出结果。
卡尔曼滤波算法C语言实现.pdf
04-03
详细讲解了卡尔曼滤波算法的每一个步骤，附有C语言代码实现，有部分笔记，以及笔记笔录，认真看应该能看懂
基于C语言的滑动平均滤波算法
最新发布
ZZLLLLLLZ的博客
07-10 149
【代码】基于C语言的滑动平均滤波算法。
数字滤波算法（一）——滑动平均滤波算法
qq_45366772的博客
04-10 3万+
学习和总结FPGA开发过程中相关数字滤波算法
（六）卡尔曼滤波（KALMAN）
qq_45574683的博客
07-22 3258
卡尔曼滤波
C语言面向对象实现滑动均值滤波与平均值滤波_滑动滤波算法c语言实现(1)
2401_84247505的博客
04-13 1021
助，可以添加V获取：vip1024b （备注Go）**[外链图片转存中…(img-3MsXcJX5-1712960222983)]
十大滤波算法总结
工大机器人工作室
07-01 1957
转载请注明出处： http://www.geek-workshop.com/thread-7694-1-1.html 1、限幅滤波法（又称程序判断滤波法） 2、中位值滤波法 3、算术平均滤波法 4、递推平均滤波法（又称滑动平均滤波法） 5、中位值平均滤波法（又称防脉冲干扰平均滤波法） 6、限幅平均滤波法 7、一阶滞后滤波法 8、加权递推平均滤波法 9、消抖滤波法 10、限幅消抖滤波法 11、新增...
C语言实现滑动平均滤波器
JeSuisDavid的博客
09-03 9191
目录前言原理代码讨论 前言 使用电机速度做闭环幻控制时，发现传感器的数据受到了高频噪声的影响，于是想先对其进行滤波处理。 原理 滑动平均滤波器本质是一个低通滤波器，与平均滤波器 代码 代码比较简单，我就没有注释。大概思想就是设置一个滤波器长度的数组，然后不断更新数组内容，再求平均就可以了。写的时候发现其实用队列写应该会更快，但是我懒了一把，因为单片机性能够用所以就算了，如果有写出来的，欢迎一起讨论哈。 /*滑动平均滤波器长度*/ #define MVF_LENGTH 8 float moving_avera
C 语言实现的滑动平均滤波算法,滑动平均滤波算法（递推平均滤波法）
weixin_30387635的博客
05-25 2135
/// ///滑动平均滤波算法(递推平均滤波法)/// /// /// GN为数组value_buf[]的元素个数,该函数主要被调用，利用参数的数组传值/// private const int GN = 12;private int filterPtr = 0;private bool isFirstGF = true;public float gSum = 0;float[] gbuf = n...
滑动平均算法
10-12
实现滑动平均算法
卡尔曼滤波算法C语言实现
09-16
不用多说，偶上传滴可都是通俗易懂滴。看看就知
卡尔曼滤波c语言实现
07-31
智能车竞赛中使用到的卡尔曼滤波程序，c语言实现，可直接使用
c代码-滑动滤波C语言实现
07-16
c代码-滑动滤波C语言实现
互补滤波与卡尔曼滤波_互补滤波与卡尔曼滤波对比解释_互补滤波_经典的文章_
10-03
文章“互补滤波与卡尔曼滤波对比介绍”很可能详细讨论了这两种滤波器的原理、实现步骤、优缺点以及适用场景。它可能还包含了一些实例分析，展示了在特定情境下如何选择合适的滤波器。通过阅读这篇经典文章，读者可以...
S7-200SMART PLC滑动平均值滤波程序
01-13
本文将详细介绍如何在S7-200SMART PLC中实现滑动平均值滤波程序，并探讨其工作原理和优势。 滑动平均值滤波是一种简单的线性滤波方法，其基本思想是通过对一定时间窗口内的连续数据进行平均，消除随机噪声，保留...
卡尔曼滤波DSP实现.zip_DSP 卡尔曼滤波_DSP 滤波_dsp卡尔曼滤波_卡尔曼_卡尔曼滤波 DSP
07-14
卡尔曼滤波是一种基于数学预测的优化估计方法，广泛应用于信号处理、控制理论和许多其他领域，尤其是在存在噪声和不确定性的情况下。在这个特定的压缩包文件中，重点是将卡尔曼滤波算法实现在数字信号处理器...
卡尔曼滤波及其改进算法、中位值平均滤波(防脉冲干扰平均滤波法)matlab程序.zip
06-26
卡尔曼滤波，扩展卡尔曼滤波，无迹卡尔曼滤波程序matlab，注释详细！适合新手！中位值平均滤波(防脉冲干扰平均滤波法)纯属看心情附赠哈哈，单独需要的可以看我的博文...
下面哪种方法不是运动目标检测或跟踪的方法：粒子滤波，卡尔曼滤波、高斯平均滤波、均值滤波
06-10
均值滤波不是运动目标检测或跟踪的方法。 粒子滤波、卡尔曼滤波和高斯平均滤波都是运动目标检测或跟踪中常用的滤波方法。其中，粒子滤波是一种基于蒙特卡罗方法的滤波算法，可用于非线性和非高斯的系统状态估计；卡尔曼滤波是一种递归滤波算法，常用于线性系统状态估计；高斯平均滤波则是基于高斯分布的滤波算法，可用于线性和非线性系统的状态估计。 这些滤波方法都可以用于运动目标检测或跟踪中，例如粒子滤波可用于目标跟踪中的状态估计，卡尔曼滤波可用于目标跟踪中的目标预测，高斯平均滤波则可用于在背景中提取目标前景。
评论
添加红包

请填写红包祝福语或标题

个

红包个数最小为10个

元

红包金额最低5元

当前余额3.43元 前往充值 >
需支付：10.00元
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包

打赏作者

全能骑士涛锅锅

你的鼓励将是我创作的最大动力

¥1 ¥2 ¥4 ¥6 ¥10 ¥20
扫码支付：¥1
获取中
扫码支付

您的余额不足，请更换扫码支付或充值

打赏作者

实付元
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明：

1.余额是钱包充值的虚拟货币，按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载，可以购买VIP、付费专栏及课程。

余额充值