1 卡尔曼滤波
详见博客 https://blog.csdn.net/moge19/article/details/81750731
2 低通滤波
2.1 算法推导
一阶RC滤波器的硬件电路如图:
图中输入电压是Vi,电阻R,电容C,输出电压为Vo。
假设电路的输出阻抗很大(即不带任何负载),输入阻抗很小(理想情况)。可以得到以下公式:
电容的阻抗是 。
而
截止频率 ,此频率下的信号,通过这个电路,输出电压和输入电压的关系式是
或者时域上的表达式:
上式离散后,可以得到:
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()