791 lines
24 KiB
C++
791 lines
24 KiB
C++
#include "Calculation.hpp"
|
||
#define PI 3.1415926535
|
||
Calculation::Calculation()
|
||
{
|
||
}
|
||
|
||
Calculation::~Calculation()
|
||
{
|
||
}
|
||
|
||
/************************************************************************/
|
||
/* 一维数据的复数快速傅里叶变换 */
|
||
/************************************************************************/
|
||
void Calculation::FFT(int n, fftw_complex* in, fftw_complex* out)
|
||
{
|
||
if (in == NULL || out == NULL) return;
|
||
fftw_plan p;
|
||
p = fftw_plan_dft_1d(n, in, out, FFTW_FORWARD, FFTW_ESTIMATE);
|
||
fftw_execute(p);
|
||
fftw_destroy_plan(p);
|
||
fftw_cleanup();
|
||
}
|
||
|
||
/************************************************************************/
|
||
/* 一维数据的实数快速傅里叶变换 */
|
||
/************************************************************************/
|
||
void Calculation::FFT_R(int n, QVector<float> & vecData, fftw_complex* out)
|
||
{
|
||
double in[n];
|
||
for (int i = 0; i < n; i++) {
|
||
in[i] = vecData[i];
|
||
}
|
||
|
||
for (int i = 0; i < n; i++)
|
||
{
|
||
out[i][0] = (double)vecData[i];
|
||
out[i][1] = 0;
|
||
}
|
||
//create a DFT plan and execute it
|
||
fftw_plan plan = fftw_plan_dft_r2c_1d(n, in, out, FFTW_ESTIMATE);
|
||
fftw_execute(plan);
|
||
//destroy the plan to prevent a memory leak
|
||
fftw_destroy_plan(plan);
|
||
fftw_cleanup();
|
||
}
|
||
|
||
/************************************************************************/
|
||
/* 一维数据的快速傅里叶逆变换 */
|
||
/************************************************************************/
|
||
void Calculation::iFFT(int n, fftw_complex* in, fftw_complex* out)
|
||
{
|
||
if (in == NULL || out == NULL) return;
|
||
fftw_plan p;
|
||
p = fftw_plan_dft_1d(n, in, out, FFTW_BACKWARD, FFTW_ESTIMATE);
|
||
fftw_execute(p);
|
||
fftw_destroy_plan(p);
|
||
fftw_cleanup();
|
||
}
|
||
|
||
//************************************
|
||
// Method: caculateAmp_Pha
|
||
// FullName: Calculation::caculateAmp_Pha
|
||
// Access: public static
|
||
// Returns: void
|
||
// Qualifier:
|
||
// Parameter: int n
|
||
// Parameter: fftw_complex * in
|
||
// Parameter: int frequency
|
||
// Parameter: double & amplitude
|
||
// Parameter: double & phase
|
||
// 函数功能是计算特定频率的幅值和相位，原来的讨论中是传入一个特定的频率，然后在给定的频率左右范围内找幅值和相位
|
||
// 目前的函数实现是计算FFT变换后特定点的幅值和相位
|
||
// 然后还有一个地方需要修改，即给定频率和FFT变换结果序列
|
||
//************************************
|
||
void Calculation::caculateAmp_Pha(int n, fftw_complex* in, int frequency, double &litude, double &phase)
|
||
{
|
||
int index = frequency;
|
||
amplitude = 2 * sqrt((in[index][0] / n) * (in[index][0] / n) + (in[index][1] / n) * (in[index][1] / n));
|
||
phase = 180 * atan(in[index][1] / in[index][0]) / M_PI;
|
||
}
|
||
|
||
|
||
void Calculation::absVec(QVector<float> & vecAbsData,QVector<float> & vecData)
|
||
{
|
||
for (int i = 0; i < vecData.size(); i++) {
|
||
vecAbsData.push_back(fabs(vecData[i]));
|
||
}
|
||
return;
|
||
}
|
||
|
||
|
||
float Calculation::mean(QVector<float> & vecData)
|
||
{
|
||
double meanTemp = 0;
|
||
for (int i = 0; i < vecData.size(); i++) {
|
||
meanTemp += (double)vecData[i];
|
||
}
|
||
return meanTemp / vecData.size();
|
||
}
|
||
|
||
|
||
void Calculation::drop_mean(QVector<float> & vecDropMeanData, QVector<float> & vecData)
|
||
{
|
||
float fMean = mean(vecData);
|
||
for (int i = 0; i < vecData.size(); i++) {
|
||
vecDropMeanData.push_back(vecData[i] - fMean);
|
||
}
|
||
return;
|
||
}
|
||
|
||
float Calculation::srm(QVector<float> & vecData)
|
||
{
|
||
double dSrmTemp = 0;
|
||
for (int i = 0; i < vecData.size(); i++){
|
||
dSrmTemp = dSrmTemp + sqrt(vecData[i]);
|
||
}
|
||
dSrmTemp = dSrmTemp / vecData.size();
|
||
return dSrmTemp * dSrmTemp;
|
||
}
|
||
|
||
|
||
float Calculation::rms(QVector<float> & vecData)
|
||
{
|
||
double rmsTemp = 0;
|
||
for (int i = 0; i < vecData.size(); i++) {
|
||
rmsTemp = rmsTemp += (vecData[i] * vecData[i]);
|
||
}
|
||
rmsTemp = rmsTemp / vecData.size();
|
||
return sqrt(rmsTemp);
|
||
}
|
||
|
||
|
||
float Calculation::variance(QVector<float> & vecDropMeanData)
|
||
{
|
||
double varianceTemp = 0;
|
||
for (int i = 0; i < vecDropMeanData.size(); i++) {
|
||
varianceTemp = varianceTemp += (vecDropMeanData[i] * vecDropMeanData[i]);
|
||
}
|
||
return varianceTemp/vecDropMeanData.size();
|
||
}
|
||
|
||
float Calculation::skew_state(QVector<float> & vecDropMeanData, float fVariance)
|
||
{
|
||
double tempSkew = 0;
|
||
for (int i = 0; i < vecDropMeanData.size(); i++) {
|
||
tempSkew = tempSkew + pow(vecDropMeanData[i], 3);
|
||
}
|
||
tempSkew = tempSkew / vecDropMeanData.size();
|
||
tempSkew = tempSkew / pow(fVariance, 1.5);
|
||
return tempSkew;
|
||
}
|
||
|
||
|
||
float Calculation::kurtosis(QVector<float> & vecDropMeanData, float fVariance)
|
||
{
|
||
double tempkurtosis = 0;
|
||
for (int i = 0; i < vecDropMeanData.size(); i++) {
|
||
tempkurtosis = tempkurtosis + pow(vecDropMeanData[i], 4);
|
||
}
|
||
tempkurtosis = tempkurtosis / vecDropMeanData.size();
|
||
tempkurtosis = tempkurtosis / pow(fVariance, 2);
|
||
return tempkurtosis;
|
||
}
|
||
|
||
void Calculation::hilbert(QVector<double> & vecData, QVector<double> & vecHilbertData, int N)
|
||
{
|
||
|
||
double in[N];
|
||
for (int i = 0; i < N; i++) {
|
||
in[i] = vecData[i];
|
||
}
|
||
fftw_complex *out;
|
||
out = (fftw_complex *)fftw_malloc(sizeof(fftw_complex) * N);
|
||
|
||
for (int i = 0; i < N; ++i)
|
||
{
|
||
out[i][0] = (double)vecData[i];
|
||
out[i][1] = 0;
|
||
}
|
||
//create a DFT plan and execute it
|
||
fftw_plan plan = fftw_plan_dft_r2c_1d(N, in, out, FFTW_ESTIMATE);
|
||
fftw_execute(plan);
|
||
//destroy the plan to prevent a memory leak
|
||
fftw_destroy_plan(plan);
|
||
int hN = N >> 1; // half of the length (N /2)
|
||
int numRem = hN; // the number of remaining elements
|
||
// multiply the appropriate values by 2
|
||
// (those that should be multiplied by 1 are left intact because they wouldn't change)
|
||
for (int i = 1; i < hN; ++i) // 1,2,...,N/2 - 1 的项乘以2
|
||
{
|
||
out[i][0] *= 2;
|
||
out[i][1] *= 2;
|
||
}
|
||
// if the length is even, the number of remaining elements decreases by 1
|
||
if (N % 2 == 0)
|
||
numRem--;
|
||
// if it's odd and greater than 1, the middle value must be multiplied by 2
|
||
else if (N > 1) // 奇数非空
|
||
{
|
||
out[hN][0] *= 2;
|
||
out[hN][1] *= 2;
|
||
}
|
||
// set the remaining values to 0
|
||
// (multiplying by 0 gives 0, so we don't care about the multiplicands)
|
||
memset(&out[hN + 1][0], 0, numRem * sizeof(fftw_complex));
|
||
// create an IDFT plan and execute it
|
||
plan = fftw_plan_dft_1d(N, out, out, FFTW_BACKWARD, FFTW_ESTIMATE);
|
||
fftw_execute(plan);
|
||
// do some cleaning
|
||
fftw_destroy_plan(plan);
|
||
fftw_cleanup();
|
||
// scale the IDFT output
|
||
for (int i = 0; i < N; ++i)
|
||
{
|
||
out[i][0] /= N;
|
||
out[i][1] /= N;
|
||
}
|
||
|
||
for( int n=0; n<N; n++ )//输出
|
||
{
|
||
// xr[n]=cos(n*pi/6);//原始信号
|
||
// y_r[n] = s_i[n];
|
||
complex complex_after;
|
||
complex_after.real = out[n][1];
|
||
complex_after.imag = out[n][0];
|
||
float amp = sqrt(complex_after.real * complex_after.real +complex_after.imag * complex_after.imag);
|
||
vecHilbertData.push_back(amp);
|
||
// printf("%d %f\n",n,vecHilbertData[n]);
|
||
}
|
||
fftw_free(out);
|
||
}
|
||
|
||
void Calculation::fftShift(fftw_complex* in, int l)
|
||
{
|
||
double temp;
|
||
double temp2;
|
||
for (int j = 0;j<l/2;j++) {
|
||
temp = in[j+l/2][0];
|
||
temp2 = in[j+l/2][1];
|
||
in[j+l/2][0] = in[j][0];
|
||
in[j+l/2][1] = in[j][1];
|
||
in[j][0] = temp;
|
||
in[j][1] = temp2;
|
||
}
|
||
}
|
||
|
||
|
||
void Calculation::FFTSpec(QVector<double> & vecData, QVector<double> & vecFFTSpecData)
|
||
{
|
||
fftw_complex *inFFt, *outFFt;
|
||
inFFt = (fftw_complex *)fftw_malloc(sizeof(fftw_complex) * vecData.size());
|
||
outFFt = (fftw_complex *)fftw_malloc(sizeof(fftw_complex) * vecData.size());
|
||
|
||
for (int j = 0; j < vecData.size(); j++) {
|
||
inFFt[j][0] = (double)vecData[j];
|
||
inFFt[j][1] = 0;
|
||
}
|
||
FFT(vecData.size(),inFFt, outFFt);
|
||
for(int j = 0; j < vecData.size()/2; j++) {
|
||
vecFFTSpecData.push_back(sqrt(outFFt[j][0]*outFFt[j][0] + outFFt[j][1]*outFFt[j][1])*2/vecData.size());
|
||
}
|
||
fftw_free(inFFt);
|
||
fftw_free(outFFt);
|
||
}
|
||
void Calculation::_iFFT( QVector<double> & vecrealData,QVector<double> & vecimageData,QVector<double> & veciFFTData)
|
||
{
|
||
fftw_complex *inFFt, *outFFt;
|
||
int N = vecrealData.size();
|
||
inFFt = (fftw_complex *)fftw_malloc(sizeof(fftw_complex) * N);
|
||
outFFt = (fftw_complex *)fftw_malloc(sizeof(fftw_complex) * N);
|
||
|
||
for (int j = 0; j < N; j++) {
|
||
inFFt[j][0] = (double)vecrealData[j];
|
||
inFFt[j][1] = (double)vecimageData[j];
|
||
}
|
||
iFFT(N,inFFt, outFFt);
|
||
for (int i = 0; i < N; i++) {
|
||
outFFt[i][0] *= 1./N;
|
||
outFFt[i][1] *= 1./N;
|
||
veciFFTData.push_back(outFFt[i][0]);
|
||
}
|
||
fftw_free(inFFt);
|
||
fftw_free(outFFt);
|
||
}
|
||
|
||
void Calculation::_FFT(QVector<double> & vecData, QVector<double> & vecFFTrealData,QVector<double> & vecFFTimageData)
|
||
{
|
||
fftw_complex *inHilFFt, *outHilFFt;
|
||
inHilFFt = (fftw_complex *)fftw_malloc(sizeof(fftw_complex) * vecData.size());
|
||
outHilFFt = (fftw_complex *)fftw_malloc(sizeof(fftw_complex) * vecData.size());
|
||
|
||
for (int j = 0; j < vecData.size(); j++) {
|
||
inHilFFt[j][0] = (double)vecData[j];
|
||
inHilFFt[j][1] = 0;
|
||
}
|
||
|
||
FFT(vecData.size(), inHilFFt, outHilFFt);
|
||
//fftShift(outHilFFt, vecData.size());
|
||
for (int i = 0; i < vecData.size(); i++) {
|
||
vecFFTrealData.push_back(outHilFFt[i][0]);
|
||
vecFFTimageData.push_back(outHilFFt[i][1]);
|
||
}
|
||
fftw_free(inHilFFt);
|
||
fftw_free(outHilFFt);
|
||
}
|
||
|
||
|
||
void Calculation::_fft(QVector<double> & vecData, QVector<double> & vecFFTrealData,QVector<double> & vecFFTimageData)
|
||
{
|
||
fftw_complex *inHilFFt, *outHilFFt;
|
||
inHilFFt = (fftw_complex *)fftw_malloc(sizeof(fftw_complex) * vecData.size());
|
||
outHilFFt = (fftw_complex *)fftw_malloc(sizeof(fftw_complex) * vecData.size());
|
||
|
||
for (int j = 0; j < vecData.size(); j++) {
|
||
inHilFFt[j][0] = (double)vecData[j];
|
||
inHilFFt[j][1] = 0;
|
||
}
|
||
|
||
FFT(vecData.size(), inHilFFt, outHilFFt);
|
||
fftShift(outHilFFt, vecData.size());
|
||
for (int i = 0; i < vecData.size(); i++) {
|
||
vecFFTrealData.push_back(outHilFFt[i][0]);
|
||
vecFFTimageData.push_back(outHilFFt[i][1]);
|
||
}
|
||
fftw_free(inHilFFt);
|
||
fftw_free(outHilFFt);
|
||
}
|
||
|
||
void Calculation::envSpec(QVector<double> & vecData, QVector<double> & vecEnvSpecData,int StartFrequency,int EndFrequency,bool PolarPlot)
|
||
{
|
||
|
||
QVector<double> vecFFTrealData,vecFFTimageData;
|
||
QVector<double> vecRealData,vecImageData;
|
||
QVector<double> veciFFtData;
|
||
QVector<double> veciFFtData2;
|
||
QVector<double> vecHilbertData;
|
||
_FFT(vecData,vecFFTrealData,vecFFTimageData);
|
||
for(int i = 0; i < vecFFTrealData.size();i++){
|
||
|
||
if(i < StartFrequency || i > EndFrequency){
|
||
vecFFTrealData.replace(i,0.0);
|
||
vecFFTimageData.replace(i,0.0);
|
||
}
|
||
}
|
||
_iFFT(vecFFTrealData,vecFFTimageData,veciFFtData);
|
||
|
||
for(int j = 0; j < veciFFtData.size();j++){
|
||
veciFFtData2.push_back(veciFFtData[j]*2);
|
||
}
|
||
|
||
if(!PolarPlot){
|
||
hilbert(veciFFtData2,vecHilbertData,veciFFtData2.size());
|
||
FFTSpec(vecHilbertData, vecEnvSpecData);
|
||
}else{
|
||
|
||
vecEnvSpecData = veciFFtData2;
|
||
}
|
||
|
||
}
|
||
//w(n) = 0.5 – 0.5*cos(2*πn/N); for n=0,1,2,…………………………N-1
|
||
void Calculation::Hanning(QVector<double> & vecData,QVector<double> & vecHanningData)
|
||
{
|
||
int N = vecData.size();
|
||
|
||
float* w = NULL;
|
||
w = (float*)calloc(N, sizeof(float));
|
||
int half, i, idx;
|
||
if (N % 2 == 0)
|
||
{
|
||
half = N / 2;
|
||
for (i = 0; i < half; i++) //CALC_HANNING Calculates Hanning window samples.
|
||
w[i] = 0.5 * (1 - cos(2 * PI * (i + 1) / (N + 1)));
|
||
|
||
|
||
idx = half - 1;
|
||
for (i = half; i < N; i++) {
|
||
w[i] = w[idx];
|
||
idx--;
|
||
}
|
||
}
|
||
else
|
||
{
|
||
half = (N + 1) / 2;
|
||
for (i = 0; i < half; i++) //CALC_HANNING Calculates Hanning window samples.
|
||
w[i] = 0.5 * (1 - cos(2 * PI * (i + 1) / (N + 1)));
|
||
|
||
idx = half - 2;
|
||
for (i = half; i < N; i++) {
|
||
w[i] = w[idx];
|
||
idx--;
|
||
}
|
||
}
|
||
for(int j = 0; j < N;j++){
|
||
vecHanningData.push_back(w[j]);
|
||
}
|
||
free(w);
|
||
}
|
||
|
||
double Calculation::Phase(QVector<double> & vecData)
|
||
{
|
||
fftw_complex *inHilFFt, *outHilFFt;
|
||
inHilFFt = (fftw_complex *)fftw_malloc(sizeof(fftw_complex) * vecData.size());
|
||
outHilFFt = (fftw_complex *)fftw_malloc(sizeof(fftw_complex) * vecData.size());
|
||
|
||
for (int j = 0; j < vecData.size(); j++) {
|
||
inHilFFt[j][0] = (double)vecData[j];
|
||
inHilFFt[j][1] = 0;
|
||
}
|
||
|
||
FFT(vecData.size(), inHilFFt, outHilFFt);
|
||
QVector<double> vecFFTrealData;
|
||
QVector<double> vecFFTimageData;
|
||
for (int i = 0; i < vecData.size(); i++) {
|
||
vecFFTrealData.push_back(outHilFFt[i][0]);
|
||
vecFFTimageData.push_back(outHilFFt[i][1]);
|
||
}
|
||
fftw_free(inHilFFt);
|
||
fftw_free(outHilFFt);
|
||
|
||
double Phase1 = atan2(vecFFTrealData[0],vecFFTimageData[0]) *180/PI;
|
||
double Phase2 = atan2(vecFFTrealData[vecData.size()-1],vecFFTimageData[vecData.size()-1]) *180/PI;
|
||
return Phase2 - Phase1;
|
||
}
|
||
|
||
void Calculation::_Integration(QVector<double> & vecData,QVector<double>& retData,double& RMS)
|
||
{
|
||
QVector<double> realshiftfft;
|
||
QVector<double> imageshiftfft;
|
||
QVector<double> realvalue,imagevalue;
|
||
_FFT(vecData,realshiftfft,imageshiftfft);
|
||
|
||
for (int i = 0; i < 10; i++) {
|
||
realshiftfft[i] = 0;
|
||
imageshiftfft[i] = 0;
|
||
}
|
||
for (int i = 1000; i < realshiftfft.size(); i++) {
|
||
realshiftfft[i] = 0;
|
||
imageshiftfft[i] = 0;
|
||
}
|
||
qDebug()<< realshiftfft.size() << endl;
|
||
qDebug()<< imageshiftfft.size() << endl;
|
||
|
||
for(int k = 1; k < realshiftfft.size()+1;k++){
|
||
realvalue.push_back((realshiftfft.at(k-1)/(k*2*PI))*1000 * 2);//单位转换mm/s,*1000 *2 精度损失
|
||
imagevalue.push_back((imageshiftfft.at(k-1)/(k*2*PI))*1000 * 2);//单位转换mm/s,*1000
|
||
}
|
||
|
||
_iFFT(realvalue,imagevalue,retData);
|
||
|
||
float Sum = 0.0;
|
||
for(int j = 0; j < retData.size();j++){
|
||
float fData = retData[j]*retData[j];
|
||
Sum += fData;
|
||
}
|
||
|
||
RMS = sqrt(Sum/(float)retData.size()); //有效值
|
||
|
||
}
|
||
|
||
void Calculation::_Differentiation(QVector<double> & vecData,QVector<double>& retData)
|
||
{
|
||
retData.push_back(vecData[0]);
|
||
for(int j = 1; j < vecData.size()-3;j++)
|
||
{
|
||
retData.push_back((vecData.at(j+1)-vecData.at(j-1))/(double)(2*1/(double)vecData.size()));
|
||
}
|
||
retData.push_back(vecData.at(vecData.size()-1));
|
||
}
|
||
|
||
QVector<double> Calculation::ComputeDenCoeffs(int FilterOrder, double Lcutoff, double Ucutoff)
|
||
{
|
||
int k; // loop variables
|
||
double theta; // PI * (Ucutoff - Lcutoff) / 2.0
|
||
double cp; // cosine of phi
|
||
double st; // sine of theta
|
||
double ct; // cosine of theta
|
||
double s2t; // sine of 2*theta
|
||
double c2t; // cosine 0f 2*theta
|
||
QVector<double> RCoeffs(2 * FilterOrder); // z^-2 coefficients
|
||
QVector<double> TCoeffs(2 * FilterOrder); // z^-1 coefficients
|
||
QVector<double> DenomCoeffs; // dk coefficients
|
||
double PoleAngle; // pole angle
|
||
double SinPoleAngle; // sine of pole angle
|
||
double CosPoleAngle; // cosine of pole angle
|
||
double a; // workspace variables
|
||
|
||
cp = cos(PI * (Ucutoff + Lcutoff) / 2.0);
|
||
theta = PI * (Ucutoff - Lcutoff) / 2.0;
|
||
st = sin(theta);
|
||
ct = cos(theta);
|
||
s2t = 2.0*st*ct; // sine of 2*theta
|
||
c2t = 2.0*ct*ct - 1.0; // cosine of 2*theta
|
||
|
||
for (k = 0; k < FilterOrder; ++k)
|
||
{
|
||
PoleAngle = PI * (double)(2 * k + 1) / (double)(2 * FilterOrder);
|
||
SinPoleAngle = sin(PoleAngle);
|
||
CosPoleAngle = cos(PoleAngle);
|
||
a = 1.0 + s2t*SinPoleAngle;
|
||
RCoeffs[2 * k] = c2t / a;
|
||
RCoeffs[2 * k + 1] = s2t*CosPoleAngle / a;
|
||
TCoeffs[2 * k] = -2.0*cp*(ct + st*SinPoleAngle) / a;
|
||
TCoeffs[2 * k + 1] = -2.0*cp*st*CosPoleAngle / a;
|
||
}
|
||
|
||
DenomCoeffs = TrinomialMultiply(FilterOrder, TCoeffs, RCoeffs);
|
||
|
||
DenomCoeffs[1] = DenomCoeffs[0];
|
||
DenomCoeffs[0] = 1.0;
|
||
for (k = 3; k <= 2 * FilterOrder; ++k)
|
||
DenomCoeffs[k] = DenomCoeffs[2 * k - 2];
|
||
|
||
for (int i = DenomCoeffs.size() - 1; i > FilterOrder * 2 + 1; i--)
|
||
DenomCoeffs.pop_back();
|
||
|
||
return DenomCoeffs;
|
||
}
|
||
|
||
QVector<double> Calculation::TrinomialMultiply(int FilterOrder, QVector<double>& b, QVector<double>& c)
|
||
{
|
||
int i, j;
|
||
QVector<double> RetVal(4 * FilterOrder);
|
||
|
||
RetVal[2] = c[0];
|
||
RetVal[3] = c[1];
|
||
RetVal[0] = b[0];
|
||
RetVal[1] = b[1];
|
||
|
||
for (i = 1; i < FilterOrder; ++i)
|
||
{
|
||
RetVal[2 * (2 * i + 1)] += c[2 * i] * RetVal[2 * (2 * i - 1)] - c[2 * i + 1] * RetVal[2 * (2 * i - 1) + 1];
|
||
RetVal[2 * (2 * i + 1) + 1] += c[2 * i] * RetVal[2 * (2 * i - 1) + 1] + c[2 * i + 1] * RetVal[2 * (2 * i - 1)];
|
||
|
||
for (j = 2 * i; j > 1; --j)
|
||
{
|
||
RetVal[2 * j] += b[2 * i] * RetVal[2 * (j - 1)] - b[2 * i + 1] * RetVal[2 * (j - 1) + 1] +
|
||
c[2 * i] * RetVal[2 * (j - 2)] - c[2 * i + 1] * RetVal[2 * (j - 2) + 1];
|
||
RetVal[2 * j + 1] += b[2 * i] * RetVal[2 * (j - 1) + 1] + b[2 * i + 1] * RetVal[2 * (j - 1)] +
|
||
c[2 * i] * RetVal[2 * (j - 2) + 1] + c[2 * i + 1] * RetVal[2 * (j - 2)];
|
||
}
|
||
|
||
RetVal[2] += b[2 * i] * RetVal[0] - b[2 * i + 1] * RetVal[1] + c[2 * i];
|
||
RetVal[3] += b[2 * i] * RetVal[1] + b[2 * i + 1] * RetVal[0] + c[2 * i + 1];
|
||
RetVal[0] += b[2 * i];
|
||
RetVal[1] += b[2 * i + 1];
|
||
}
|
||
|
||
return RetVal;
|
||
}
|
||
|
||
QVector<double> Calculation::ComputeNumCoeffs(int FilterOrder, double Lcutoff, double Ucutoff, QVector<double>& DenC)
|
||
{
|
||
QVector<double> TCoeffs;
|
||
QVector<double> NumCoeffs(2 * FilterOrder + 1);
|
||
QVector<std::complex<double>> NormalizedKernel(2 * FilterOrder + 1);
|
||
|
||
QVector<double> Numbers;
|
||
for (double n = 0; n < FilterOrder * 2 + 1; n++)
|
||
Numbers.push_back(n);
|
||
int i;
|
||
|
||
TCoeffs = ComputeHP(FilterOrder);
|
||
|
||
for (i = 0; i < FilterOrder; ++i)
|
||
{
|
||
NumCoeffs[2 * i] = TCoeffs[i];
|
||
NumCoeffs[2 * i + 1] = 0.0;
|
||
}
|
||
NumCoeffs[2 * FilterOrder] = TCoeffs[FilterOrder];
|
||
|
||
double cp[2];
|
||
double Bw, Wn;
|
||
cp[0] = 2 * 2.0*tan(PI * Lcutoff / 2.0);
|
||
cp[1] = 2 * 2.0*tan(PI * Ucutoff / 2.0);
|
||
|
||
Bw = cp[1] - cp[0];
|
||
//center frequency
|
||
Wn = sqrt(cp[0] * cp[1]);
|
||
Wn = 2 * atan2(Wn, 4);
|
||
double kern;
|
||
const std::complex<double> result = std::complex<double>(-1, 0);
|
||
|
||
for (int k = 0; k< FilterOrder * 2 + 1; k++)
|
||
{
|
||
NormalizedKernel[k] = std::exp(-sqrt(result)*Wn*Numbers[k]);
|
||
}
|
||
double b = 0;
|
||
double den = 0;
|
||
for (int d = 0; d < FilterOrder * 2 + 1; d++)
|
||
{
|
||
b += real(NormalizedKernel[d] * NumCoeffs[d]);
|
||
den += real(NormalizedKernel[d] * DenC[d]);
|
||
}
|
||
for (int c = 0; c < FilterOrder * 2 + 1; c++)
|
||
{
|
||
NumCoeffs[c] = (NumCoeffs[c] * den) / b;
|
||
}
|
||
|
||
for (int i = NumCoeffs.size() - 1; i > FilterOrder * 2 + 1; i--)
|
||
NumCoeffs.pop_back();
|
||
|
||
return NumCoeffs;
|
||
}
|
||
|
||
QVector<double> Calculation::ComputeLP(int FilterOrder)
|
||
{
|
||
QVector<double> NumCoeffs(FilterOrder + 1);
|
||
int m;
|
||
int i;
|
||
|
||
NumCoeffs[0] = 1;
|
||
NumCoeffs[1] = FilterOrder;
|
||
m = FilterOrder / 2;
|
||
for (i = 2; i <= m; ++i)
|
||
{
|
||
NumCoeffs[i] = (double)(FilterOrder - i + 1)*NumCoeffs[i - 1] / i;
|
||
NumCoeffs[FilterOrder - i] = NumCoeffs[i];
|
||
}
|
||
NumCoeffs[FilterOrder - 1] = FilterOrder;
|
||
NumCoeffs[FilterOrder] = 1;
|
||
|
||
return NumCoeffs;
|
||
}
|
||
|
||
QVector<double> Calculation::ComputeHP(int FilterOrder)
|
||
{
|
||
QVector<double> NumCoeffs;
|
||
int i;
|
||
|
||
NumCoeffs = ComputeLP(FilterOrder);
|
||
|
||
for (i = 0; i <= FilterOrder; ++i)
|
||
if (i % 2) NumCoeffs[i] = -NumCoeffs[i];
|
||
|
||
return NumCoeffs;
|
||
}
|
||
|
||
//vector<double> filter(int ord, vector<double> a, vector<double> b, vector<double> x)
|
||
//{
|
||
// int np = x.size();
|
||
// vector<double> y(np);
|
||
//
|
||
// int i, j;
|
||
// y[0] = b[0] * x[0];
|
||
// for (i = 1; i<ord + 1; i++)
|
||
// {
|
||
// y[i] = 0.0;
|
||
// for (j = 0; j<i + 1; j++)
|
||
// y[i] = y[i] + b[j] * x[i - j];
|
||
// for (j = 0; j<i; j++)
|
||
// y[i] = y[i] - a[j + 1] * y[i - j - 1];
|
||
// }
|
||
// for (i = ord + 1; i<np + 1; i++)
|
||
// {
|
||
// y[i] = 0.0;
|
||
// for (j = 0; j<ord + 1; j++)
|
||
// y[i] = y[i] + b[j] * x[i - j];
|
||
// for (j = 0; j<ord; j++)
|
||
// y[i] = y[i] - a[j + 1] * y[i - j - 1];
|
||
// }
|
||
//
|
||
// return y;
|
||
//}
|
||
|
||
QVector<double> Calculation::filter(QVector<double>&x, QVector<double>& coeff_b, QVector<double>& coeff_a)
|
||
{
|
||
int len_x = x.size();
|
||
int len_b = coeff_b.size();
|
||
int len_a = coeff_a.size();
|
||
|
||
QVector<double> zi(len_b);
|
||
|
||
QVector<double> filter_x(len_x);
|
||
|
||
if (len_a == 1)
|
||
{
|
||
for (int m = 0; m<len_x; m++)
|
||
{
|
||
filter_x[m] = coeff_b[0] * x[m] + zi[0];
|
||
for (int i = 1; i<len_b; i++)
|
||
{
|
||
zi[i - 1] = coeff_b[i] * x[m] + zi[i];//-coeff_a[i]*filter_x[m];
|
||
}
|
||
}
|
||
}
|
||
else
|
||
{
|
||
for (int m = 0; m<len_x; m++)
|
||
{
|
||
filter_x[m] = coeff_b[0] * x[m] + zi[0];
|
||
for (int i = 1; i<len_b; i++)
|
||
{
|
||
zi[i - 1] = coeff_b[i] * x[m] + zi[i] - coeff_a[i] * filter_x[m];
|
||
}
|
||
}
|
||
}
|
||
|
||
return filter_x;
|
||
}
|
||
|
||
//vector<double> bandpass(vector<double> input, double lowpass, double highpass, double fps)
|
||
//{
|
||
// double N = input.size();
|
||
// cv::Mat x = cv::Mat::zeros(1, input.size(), CV_64FC1);
|
||
//
|
||
// for (int i = 0; i < input.size(); i++)
|
||
// {
|
||
// x.at<double>(0, i) = input[i];
|
||
// }
|
||
//
|
||
// Mat x_fre;
|
||
// dft(x, x_fre, DFT_COMPLEX_OUTPUT);
|
||
//
|
||
// Mat W = Mat::zeros(1, input.size(), CV_64FC1);
|
||
//
|
||
// for (int i = 0; i < input.size(); i++)
|
||
// {
|
||
// if ((double)i / N *)
|
||
// }
|
||
//}
|
||
|
||
void Calculation::ButterWorth(QVector<double>&inData,double *FrequencyBands,QVector<double>& outData)
|
||
{
|
||
int FiltOrd = 4;
|
||
// double s = inData.size();
|
||
// double a = cos(M_PI*(FrequencyBands[0]+FrequencyBands[1])/s)/cos(M_PI*(FrequencyBands[0]-FrequencyBands[1])/s);
|
||
// double a2 = a*a;
|
||
// double b = tan(M_PI*(FrequencyBands[0]-FrequencyBands[1])/s);
|
||
// double b2 = b*b;
|
||
// double r;
|
||
// int n = FiltOrd/4;
|
||
// double *A = (double *)malloc(n*sizeof(double));
|
||
// double *d1 = (double *)malloc(n*sizeof(double));
|
||
// double *d2 = (double *)malloc(n*sizeof(double));
|
||
// double *d3 = (double *)malloc(n*sizeof(double));
|
||
// double *d4 = (double *)malloc(n*sizeof(double));
|
||
// double *w0 = (double *)calloc(n, sizeof(double));
|
||
// double *w1 = (double *)calloc(n, sizeof(double));
|
||
// double *w2 = (double *)calloc(n, sizeof(double));
|
||
// double *w3 = (double *)calloc(n, sizeof(double));
|
||
// double *w4 = (double *)calloc(n, sizeof(double));
|
||
// double x;
|
||
|
||
// for(int i=0; i<n; ++i){
|
||
// r = sin(M_PI*(2.0*i+1.0)/(4.0*n));
|
||
// s = b2 + 2.0*b*r + 1.0;
|
||
// A[i] = 1.0/s;
|
||
// d1[i] = 4.0*a*(1.0+b*r)/s;
|
||
// d2[i] = 2.0*(b2-2.0*a2-1.0)/s;
|
||
// d3[i] = 4.0*a*(1.0-b*r)/s;
|
||
// d4[i] = -(b2 - 2.0*b*r + 1.0)/s;}
|
||
// r = 4.0*a;
|
||
// s = 4.0*a2+2.0;
|
||
|
||
// for(int j = 0; j < inData.size(); j++){
|
||
// for(int i=0; i<n; ++i){
|
||
// w0[i] = d1[i]*w1[i] + d2[i]*w2[i]+ d3[i]*w3[i]+ d4[i]*w4[i] + inData[j];
|
||
// x = A[i]*(w0[i] - r*w1[i] + s*w2[i]- r*w3[i] + w4[i]);
|
||
// w4[i] = w3[i];
|
||
// w3[i] = w2[i];
|
||
// w2[i] = w1[i];
|
||
// w1[i] = w0[i];
|
||
// }
|
||
// outData.push_back(x);
|
||
// //printf("%lf\n", x);
|
||
// }
|
||
|
||
// free(A);free(A);free(d1);free(d2);free(d3);free(d4);
|
||
// free(w0);free(w1);free(w2);free(w3);free(w4);
|
||
QVector<double> a,b;
|
||
|
||
a = ComputeDenCoeffs(FiltOrd, FrequencyBands[0], FrequencyBands[1]);
|
||
|
||
b = ComputeNumCoeffs(FiltOrd, FrequencyBands[0], FrequencyBands[1], a);
|
||
|
||
outData = filter(inData,b,a);
|
||
}
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|