全能骑士涛锅锅

全能骑士涛锅锅

关注
发信
关注(28)粉丝(399)

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

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

前言

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

卡尔曼滤波起源于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);
09-06 07:50
Powered by LMLPHP ©2024 全能骑士涛锅锅 0.004841