1 卡尔曼滤波
详见博客 https://blog.csdn.net/moge19/article/details/81750731
2 低通滤波
2.1 算法推导
一阶RC滤波器的硬件电路如图:
图中输入电压是Vi,电阻R,电容C,输出电压为Vo。
假设电路的输出阻抗很大(即不带任何负载),输入阻抗很小(理想情况)。可以得到以下公式:
V o = 1 1 + j ω R C V i V_o = \frac{1}{1+j\omega RC}V_i Vo=1+jωRC1Vi
电容的阻抗是 Z C = 1 j ω C Z_C = \frac{1}{j\omega C} ZC=jωC1。
而 ω = 2 π f \omega = 2\pi f ω=2πf
截止频率 f c u t = 1 2 π R C f_{cut} = \frac{1}{2\pi RC} fcut=2πRC1,此频率下的信号,通过这个电路,输出电压和输入电压的关系式是 V o = 1 1 + j V i V_o = \frac{1}{1+j}V_i Vo=1+j1Vi
或者时域上的表达式:
V o = V i − R C d V o d t V_o = V_i - RC\frac{dV_o}{dt} Vo=Vi−RCdtdVo
上式离散后,可以得到:
V o ( k ) = V i ( k ) + R C T s V o ( k − 1 ) 1 + R C T s V_o\left ( k \right )=\frac{V_i\left ( k \right )+\frac{RC}{T_s}V_o\left ( k-1 \right )}{1+\frac{RC}{T_s}} Vo(k)=1+TsRCVi(k)+TsRCVo(k−1)
2.2 算法实现
class RC_filter:
def __init__(self,sampleFrq,CutFrq):
self.sampleFrq = sampleFrq
self.CutFrq = CutFrq
self.adc_old=0
def LowPassFilter_RC_1order(self,Vi):
RC = 1.0/2.0/math.pi/self.CutFrq
Cof1 = 1/(1+RC * self.sampleFrq)
Cof2 = RC* self.sampleFrq/(1+RC* self.sampleFrq)
Vo = Cof1 * Vi + Cof2 * self.adc_old
self.adc_old = Vo
return Vo
3 滑动平均滤波
算法实现:
noise_array = np.random.normal(0, 400, noise_size)
test_array_mean = [0]*32
mean_array = []
for i in range(noise_size):
test_array_mean[i%32] = noise_array[i]
mean_array.append(sum(test_array_mean)/32)
4 FFT转换比较
源数据频谱图:
卡尔曼滤波后的频谱
RC滤波后的频谱
从图中可以看出一阶的卡尔曼滤波与RC低通滤波的滤波效果一样
滑动平均滤波后的频谱图:
从图中可以看出,滑动平均滤波,在低频段较之卡尔曼滤波和低通滤波,其频谱得到的幅值是其余二者的数十倍,这是由于求均值时,将部分噪声数据也计算到信号数据中导致的,因此可以看出,滑动平均滤波不能有效得将噪声数据滤掉,不过在工程因为计算简单,浮点运算少,甚至没有,所以得到广泛运用。
5 实现代码
# -*- coding: utf-8 -*-
"""
Created on Fri Feb 15 20:35:39 2019
@author: ASUS
"""
import numpy as np
import matplotlib.pyplot as plt
import math
from scipy.fftpack import fft,ifft
class kalman_filter:
def __init__(self,Q,R):
self.Q = Q
self.R = R
self.P_k_k1 = 1
self.Kg = 0
self.P_k1_k1 = 1
self.x_k_k1 = 0
self.ADC_OLD_Value = 0
self.Z_k = 0
self.kalman_adc_old=0
def kalman(self,ADC_Value):
self.Z_k = ADC_Value
#if (abs(self.kalman_adc_old-ADC_Value)>=60):
#self.x_k1_k1= ADC_Value*0.382 + self.kalman_adc_old*0.618
#else:
self.x_k1_k1 = self.kalman_adc_old;
self.x_k_k1 = self.x_k1_k1
self.P_k_k1 = self.P_k1_k1 + self.Q
self.Kg = self.P_k_k1/(self.P_k_k1 + self.R)
kalman_adc = self.x_k_k1 + self.Kg * (self.Z_k - self.kalman_adc_old)
self.P_k1_k1 = (1 - self.Kg)*self.P_k_k1
self.P_k_k1 = self.P_k1_k1
self.kalman_adc_old = kalman_adc
return kalman_adc
class RC_filter:
def __init__(self,sampleFrq,CutFrq):
self.sampleFrq = sampleFrq
self.CutFrq = CutFrq
self.adc_old=0
def LowPassFilter_RC_1order(self,Vi):
RC = 1.0/2.0/math.pi/self.CutFrq
Cof1 = 1/(1+RC * self.sampleFrq)
Cof2 = RC* self.sampleFrq/(1+RC* self.sampleFrq)
Vo = Cof1 * Vi + Cof2 * self.adc_old
self.adc_old = Vo
return Vo
if __name__ == '__main__':
noise_size = 1024
noise_size_half = 512
kalman_filter = kalman_filter(0.01,0.5)
RC_filter = RC_filter(400,5)
noise_array = np.random.normal(0, 2, noise_size)
adc_value=[]
for i in range(noise_size):
adc_value.append(0)
adc_value_noise = np.array(adc_value) + noise_array
adc_filter_1=[]
for i in range(noise_size):
adc_filter_1.append(kalman_filter.kalman(adc_value_noise[i]))
plt.plot(adc_value_noise,'r')
plt.plot(adc_filter_1,'b')
#plt.plot(test_array)
plt.show()
adc_filter_2=[]
plt.figure(1)
for i in range(noise_size):
adc_filter_2.append(RC_filter.LowPassFilter_RC_1order(adc_value_noise[i]))
plt.plot(adc_value_noise,'r-')
plt.plot(adc_filter_2,'b')
#plt.plot(test_array)
plt.show()
plt.figure(2)
x = range(noise_size)
y = noise_array
#x=np.linspace(0,1,1400)
#设置需要采样的信号,频率分量有180,390和600
#y=7*np.sin(2*np.pi*180*x) + 2.8*np.sin(2*np.pi*390*x)+5.1*np.sin(2*np.pi*600*x)
yy=fft(x) #快速傅里叶变换
yreal = yy.real # 获取实数部分
yimag = yy.imag # 获取虚数部分
yf=abs(fft(y)) # 取绝对值
yf1=abs(fft(y))/len(x) #归一化处理
yf2 = yf1[range(int(len(x)/2))] #由于对称性,只取一半区间
xf = np.arange(len(y)) # 频率
xf1 = xf
xf2 = xf[range(int(len(x)/2))] #取一半区间
"""plt.subplot(221)
plt.plot(x[0:noise_size_half],y[0:noise_size_half])
plt.title('Original wave')
plt.subplot(222)
plt.plot(xf,yf,'r')
plt.title('FFT of Mixed wave(two sides frequency range)',fontsize=7,color='#7A378B') #注意这里的颜色可以查询颜色代码表
plt.subplot(223)
plt.plot(xf1,yf1,'g')
plt.title('FFT of Mixed wave(normalization)',fontsize=9,color='r') """
#plt.subplot(224)
plt.plot(xf2,yf2,'b')
plt.title('FFT of kalman_filter)',fontsize=10,color='#F08080')
plt.show()
x = range(noise_size)
y = adc_filter_2
#x=np.linspace(0,1,1400)
#设置需要采样的信号,频率分量有180,390和600
#y=7*np.sin(2*np.pi*180*x) + 2.8*np.sin(2*np.pi*390*x)+5.1*np.sin(2*np.pi*600*x)
yy = fft(x) #快速傅里叶变换
yreal = yy.real # 获取实数部分
yimag = yy.imag # 获取虚数部分
yf =abs(fft(y)) # 取绝对值
yf1=abs(fft(y))/len(x) #归一化处理
yf2= yf1[range(int(len(x)/2))] #由于对称性,只取一半区间
xf = np.arange(len(y)) # 频率
xf1 = xf
xf2 = xf[range(int(len(x)/2))] #取一半区间
"""plt.subplot(221)
plt.plot(x[0:noise_size_half],y[0:noise_size_half])
plt.title('Original wave')
plt.subplot(222)
plt.plot(xf,yf,'r')
plt.title('FFT of Mixed wave(two sides frequency range)',fontsize=7,color='#7A378B') #注意这里的颜色可以查询颜色代码表
plt.subplot(223)
plt.plot(xf1,yf1,'g')
plt.title('FFT of Mixed wave(normalization)',fontsize=9,color='r') """
#plt.subplot(224)
plt.plot(xf2,yf2,'b')
plt.title('FFT of RC)',fontsize=10,color='#F08080')
plt.show()