535 lines
16 KiB
C++
535 lines
16 KiB
C++
#include "Calculation.hpp"
|
||
|
||
|
||
|
||
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, std::vector<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();
|
||
}
|
||
void Calculation::_iFFT( std::vector<float> & vecrealData,std::vector<float> & vecimageData,std::vector<float> & 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(std::vector<float> & vecData, std::vector<float> & vecFFTrealData,std::vector<float> & 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);
|
||
}
|
||
//************************************
|
||
// 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;
|
||
}
|
||
|
||
|
||
float Calculation::max(std::vector<float> & vecData)
|
||
{
|
||
std::vector<float>::iterator it;
|
||
it = std::max_element(vecData.begin(), vecData.end());
|
||
if (it != vecData.end()) {
|
||
return *it;
|
||
}
|
||
return 0;
|
||
}
|
||
|
||
float Calculation::min(std::vector<float> & vecData)
|
||
{
|
||
std::vector<float>::iterator it;
|
||
it = std::min_element(vecData.begin(), vecData.end());
|
||
if (it != vecData.end()) {
|
||
return *it;
|
||
}
|
||
return 0;
|
||
}
|
||
|
||
|
||
void Calculation::absVec(std::vector<float> & vecAbsData, std::vector<float> & vecData)
|
||
{
|
||
for (int i = 0; i < vecData.size(); i++) {
|
||
vecAbsData.push_back(fabs(vecData[i]));
|
||
}
|
||
return;
|
||
}
|
||
|
||
|
||
float Calculation::mean(std::vector<float> & vecData)
|
||
{
|
||
double meanTemp = 0;
|
||
for (int i = 0; i < vecData.size(); i++) {
|
||
meanTemp = meanTemp += vecData[i];
|
||
}
|
||
return meanTemp / vecData.size();
|
||
}
|
||
|
||
|
||
void Calculation::drop_mean(std::vector<float> & vecDropMeanData, std::vector<float> & vecData)
|
||
{
|
||
float fMean = mean(vecData);
|
||
for (int i = 0; i < vecData.size(); i++) {
|
||
vecDropMeanData.push_back(vecData[i] - fMean);
|
||
}
|
||
return;
|
||
}
|
||
|
||
float Calculation::srm(std::vector<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(std::vector<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::getSample_variance(std::vector<float> a)
|
||
{
|
||
float ss = 0;
|
||
float s = 0;
|
||
float mx = mean(a);
|
||
int len = a.size();
|
||
for (int i = 0; i < len; i++) {
|
||
s = a[i] - mx;
|
||
ss += pow(s, 2);
|
||
}
|
||
return ss / (len - 1);
|
||
}
|
||
|
||
|
||
|
||
float Calculation::variance(std::vector<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(std::vector<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(std::vector<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::Hanning(std::vector<float> & vecData,std::vector<float> & 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);
|
||
}
|
||
void Calculation::hilbert(std::vector<float> & vecData, std::vector<float> & 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(std::vector<float> & vecData, std::vector<float> & 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());
|
||
printf("1---------------------------------------------->%d\n",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());
|
||
}
|
||
|
||
}
|
||
|
||
void Calculation::envSpec(std::vector<float> & vecData, std::vector<float> & vecEnvSpecData,int StartFrequency,int EndFrequency)
|
||
{
|
||
/*std::vector<float> vecDropMeanData;
|
||
drop_mean(vecDropMeanData, vecData);
|
||
|
||
std::vector<float> vecHilbertData;
|
||
hilbert(vecDropMeanData, vecHilbertData, vecDropMeanData.size());
|
||
|
||
fftw_complex *inHilFFt, *outHilFFt;
|
||
inHilFFt = (fftw_complex *)fftw_malloc(sizeof(fftw_complex) * vecHilbertData.size());
|
||
outHilFFt = (fftw_complex *)fftw_malloc(sizeof(fftw_complex) * vecHilbertData.size());
|
||
|
||
for (int j = 0; j < vecHilbertData.size(); j++) {
|
||
inHilFFt[j][0] = (double)vecHilbertData[j];
|
||
inHilFFt[j][1] = 0;
|
||
}
|
||
|
||
FFT(vecHilbertData.size(), inHilFFt, outHilFFt);
|
||
fftShift(outHilFFt, vecHilbertData.size());
|
||
|
||
vecEnvSpecData.push_back(0);
|
||
for (int i = vecHilbertData.size() / 2 + 1; i < vecHilbertData.size(); i++) {
|
||
float ftemp = 2 * sqrt(outHilFFt[i][0] * outHilFFt[i][0] + outHilFFt[i][1] * outHilFFt[i][1]) / vecHilbertData.size();
|
||
vecEnvSpecData.push_back(ftemp);
|
||
}*/
|
||
std::vector<float> vecFFTrealData,vecFFTimageData;
|
||
std::vector<float> vecRealData,vecImageData;
|
||
std::vector<float> veciFFtData;
|
||
std::vector<float> veciFFtData2;
|
||
std::vector<float> vecHilbertData;
|
||
_FFT(vecData,vecFFTrealData,vecFFTimageData);
|
||
for(int i = 0; i < vecFFTrealData.size();i++){
|
||
if(i > StartFrequency && i < EndFrequency){
|
||
vecRealData.push_back(vecFFTrealData.at(i));
|
||
vecImageData.push_back(vecFFTimageData.at(i));
|
||
}else{
|
||
vecRealData.push_back(0);
|
||
vecImageData.push_back(0);
|
||
}
|
||
}
|
||
_iFFT(vecRealData,vecImageData,veciFFtData);
|
||
for(int j = 0; j < veciFFtData.size();j++){
|
||
veciFFtData2.push_back(veciFFtData[j]*2);
|
||
}
|
||
hilbert(veciFFtData2,vecHilbertData,veciFFtData2.size());
|
||
FFTSpec(vecHilbertData, vecEnvSpecData);
|
||
}
|
||
|
||
|
||
void Calculation::GenerateSin(std::vector<float> & vecData)
|
||
{
|
||
double frequency = 800.0; // Frequency of the sine wave in Hz
|
||
double sampling_rate = 12800.0; // Sampling rate in Hz (8 kHz)
|
||
size_t num_samples = 12800; // Total number of samples (1 second of data)
|
||
double dt = 1.0 / sampling_rate; // Time step in seconds
|
||
|
||
// Vector to hold the sine wave data
|
||
//std::vector<double> sine_wave(num_samples);
|
||
|
||
// Generate the sine wave
|
||
for (size_t i = 0; i < num_samples; ++i) {
|
||
vecData.push_back(std::sin(2 * M_PI * frequency * i * dt));
|
||
}
|
||
|
||
}
|
||
|
||
void Calculation::Integration(std::vector<float> & vecData,std::vector<float>& retData,double & resolution)
|
||
{
|
||
|
||
std::vector<float> realshiftfft;
|
||
std::vector<float> imageshiftfft;
|
||
std::vector<float> realvalue,imagevalue;
|
||
_FFT(vecData,realshiftfft,imageshiftfft);
|
||
|
||
for (int i = 0; i < 5 / resolution; i++) {
|
||
realshiftfft[i] = 0;
|
||
imageshiftfft[i] = 0;
|
||
}
|
||
for (int i = 1000 / resolution ; i < realshiftfft.size(); i++) {
|
||
realshiftfft[i] = 0;
|
||
imageshiftfft[i] = 0;
|
||
}
|
||
|
||
for(int k = 1; k < realshiftfft.size()+1;k++){
|
||
realvalue.push_back((realshiftfft.at(k-1)/(k*2*M_PI))*1000 * 2);//单位转换mm/s,*1000 *2 精度损失
|
||
imagevalue.push_back((imageshiftfft.at(k-1)/(k*2*M_PI))*1000 * 2);//单位转换mm/s,*1000
|
||
}
|
||
|
||
_iFFT(realvalue,imagevalue,retData);
|
||
|
||
|
||
}
|
||
|
||
/*void acceleration_to_velocity(int fs, int N, float *data, int min_freq, int max_freq, float *out_data) {
|
||
fftwf_execute_dft_r2c(forward_plan[fft_plan_id(N)], data, forward_out);
|
||
|
||
float df = fs * 1.0f / N;
|
||
int ni = round(min_freq / df);
|
||
int na = round(max_freq / df);
|
||
float dw = 2 * fcl_pi * df;
|
||
|
||
int double_id = get_idle_double_res();
|
||
if (double_id < 0) {
|
||
return;
|
||
}
|
||
float *w11 = get_double_res_ptr(double_id);
|
||
|
||
int len = 0;
|
||
int max_f = round((0.5 * fs - df) / df);
|
||
|
||
for (int i = 0; i <= max_f; ++i) {
|
||
w11[i] = i * dw;
|
||
++len;
|
||
}
|
||
|
||
for (int i = 0; i < max_f; ++i) {
|
||
w11[len] = -2 * fcl_pi * (0.5 * fs - df) + i * dw;
|
||
++len;
|
||
}
|
||
|
||
forward_out[0][0] = forward_out[0][1] = forward_out[N - 1][0] = forward_out[N - 1][1] = 0;
|
||
float tmp_real, tmp_imag;
|
||
for (int i = 1; i < N - 1; ++i) {
|
||
tmp_real = forward_out[i][1] / w11[i];
|
||
tmp_imag = -forward_out[i][0] / w11[i];
|
||
forward_out[i][0] = tmp_real; // real
|
||
forward_out[i][1] = tmp_imag; // imag
|
||
}
|
||
|
||
free_double_res(double_id);
|
||
|
||
for (int i = 0; i < N; ++i) {
|
||
backward_in[i][0] = 0;
|
||
backward_in[i][1] = 0;
|
||
}
|
||
|
||
for (int i = ni - 1; i < na; ++i) {
|
||
backward_in[i][0] = forward_out[i][0];
|
||
backward_in[i][1] = forward_out[i][1];
|
||
}
|
||
|
||
for (int i = N - na; i < N - ni + 1; ++i) {
|
||
backward_in[i][0] = forward_out[i][0];
|
||
backward_in[i][1] = forward_out[i][1];
|
||
}
|
||
|
||
fftwf_execute_dft(backward_plan[fft_plan_id(N)], backward_in, backward_out);
|
||
|
||
for (int i = 0; i < N; ++i) {
|
||
out_data[i] = backward_out[i][0] / N * 2 * 1000; // *1000 是单位换算, 跟python保持一致
|
||
}
|
||
}*/ |