void RobotState::DVLCallback(const underwater_sensor_msgs::DVLConstPtr &message)
{
cv::Point3f white_noise(var_nor(),var_nor(),var_nor());
float elapsed_time;
if(message->bi_error < 1)
{
vel_dvl_.x = message->bi_x_axis;
vel_dvl_.y = message->bi_y_axis;
vel_dvl_.z = message->bi_z_axis;
//vel_dvl_ += white_noise;
//ROS_DEBUG_STREAM_NAMED("rs","DVL velocity = " << vel_dvl_);
if(has_dvl_ && has_imu_)
{
elapsed_time = (message->header.stamp - timestamp_).toSec();
computeTraveledDistances(elapsed_time);
}
timestamp_ = message->header.stamp;
has_dvl_ = true;
}
else
{
ROS_DEBUG_STREAM_NAMED("rs","O erro da DVL eh " << message->bi_error);
}
}
//----
//
//----
void wnoise_tilde_bang(t_wnoise_tilde *x) {
int i;
x->read_index = 0; // reset read index
for (i = 0; i < x->noise_buff_length; i++) {
x->noise_buff[i] = white_noise();
x->wnoise_tilde_sig = x->noise_buff[i];
}
}
void mpg123_noise(float* table, size_t count, enum mpg123_noise_type noisetype)
{
switch(noisetype)
{
case mpg123_white_noise: white_noise(table, count); break;
case mpg123_tpdf_noise: tpdf_noise(table, count); break;
case mpg123_highpass_tpdf_noise:
highpass_tpdf_noise(table, count);
break;
}
}
static FP_TYPE* synth_noise(llsm_parameters param, llsm_conf conf, FP_TYPE** wrapped_spectrogram, int winsize) {
/*
To suppress glitches caused by aliasing, we apply MLT sine window twice, before analysis and after synthesis respectively;
Overlapping factor should be greater or equal to 4 for MLT sine window;
To preserve time resolution, we actually lower the hopsize by half, meanwhile double sampling wrapped_spectrogram.
*/
conf.nhop /= 2;
int nfft = winsize * conf.nhop * 2;
int ny = conf.nfrm * conf.nhop * 2 + nfft * 16;
FP_TYPE* y = calloc(ny, sizeof(FP_TYPE));
FP_TYPE* w = hanning(nfft);
FP_TYPE* realbuff = calloc(nfft, sizeof(FP_TYPE));
FP_TYPE* imagbuff = calloc(nfft, sizeof(FP_TYPE));
FP_TYPE* yfrm = calloc(nfft, sizeof(FP_TYPE));
FP_TYPE fftbuff[65536];
FP_TYPE norm_factor = 0.5 * sumfp(w, nfft);
FP_TYPE norm_factor_win = 0;
{
int i = 0;
while(i < nfft) {
norm_factor_win += w[i];
i += conf.nhop;
}
}
for(int j = 0; j < nfft; j ++)
w[j] = sqrt(w[j]);
FP_TYPE* x = white_noise(1.0, ny);
FP_TYPE* freqwrap = llsm_wrap_freq(0, conf.nosf, conf.nnos, conf.noswrap);
for(int i = 0; i < conf.nfrm * 2; i ++) {
int t = i * conf.nhop;
FP_TYPE* spec = llsm_spectrum_from_envelope(freqwrap, wrapped_spectrogram[i / 2], conf.nnos, nfft / 2, param.s_fs);
FP_TYPE* xfrm = fetch_frame(x, ny, t, nfft);
for(int j = 0; j < nfft; j ++)
xfrm[j] *= w[j];
fft(xfrm, NULL, realbuff, imagbuff, nfft, fftbuff);
for(int j = 0; j < nfft / 2; j ++) {
FP_TYPE a = fastexp(spec[j]) * norm_factor; // amplitude
FP_TYPE p = fastatan2(imagbuff[j], realbuff[j]);
realbuff[j] = a * cos(p);
imagbuff[j] = a * sin(p);
}
complete_symm (realbuff, nfft);
complete_asymm(imagbuff, nfft);
ifft(realbuff, imagbuff, yfrm, NULL, nfft, fftbuff);
for(int j = 0; j < nfft; j ++) {
int idx = t + j - nfft / 2;
if(idx >= 0)
y[idx] += yfrm[j] * w[j] / norm_factor_win;
}
free(spec);
free(xfrm);
}
free(w);
free(x);
free(yfrm);
free(realbuff);
free(imagbuff);
free(freqwrap);
return y;
}
int main(int argc, char **argv)
{
int k, t, s;
double *x, *z, *a, *e, *u;
// the number of time series data
int max_points = 10000;
int outlier_point;
// ar coefficient parameter
double ar1_coeff = 0.6;
double ar2_coeff = 0.5;
// change point params
int change_point_time = 1000;
double expected_change_total = 5.0;
double decrease_change_value = 0.5;
// white noise params
double expected_value = 0.0;
double variance_value = 1.0;
srand((unsigned)time(NULL));
outlier_point = rand() % max_points;
if (argc != 9) {
fprintf(stderr, "Usage : %s number_of_data ar1_coeff ar2_coeff "
"change_point_time change_point_total "
"change_point_decrease expected_value_for_white_noise "
"variance_value_white_noise\n",
argv[0]);
exit(1);
}
if ((x = (double *)malloc((max_points + 1) * sizeof(double))) == NULL)
goto MALLOC_ERROR;
if ((z = (double *)malloc((max_points + 1) * sizeof(double))) == NULL)
goto MALLOC_ERROR;
if ((a = (double *)malloc((k + 1) * sizeof(double))) == NULL)
goto MALLOC_ERROR;
if ((e = (double *)malloc((max_points + 1) * sizeof(double))) == NULL)
goto MALLOC_ERROR;
if ((u = (double *)malloc((max_points + 1) * sizeof(double))) == NULL)
goto MALLOC_ERROR;
// ar coefficient order
k = 2;
// the number of time series data
max_points = atoi(argv[1]);
ar1_coeff = atof(argv[2]);
ar2_coeff = atof(argv[3]);
change_point_time = atoi(argv[4]);
expected_change_total = atof(argv[5]);
decrease_change_value = atof(argv[6]);
expected_value = atof(argv[7]);
variance_value = atof(argv[8]);
// setup ar coefficient
a[0] = 0;
a[1] = ar1_coeff;
a[2] = ar2_coeff;
// Change Points Initial Value
u[0] = expected_change_total;
t = 1;
s = 1;
while (t <= max_points) {
if (t > 2) {
u[t] = u[t - 1];
e[t] = white_noise(expected_value, variance_value);
z[t] = a[1] * z[t - 1] - a[2] * z[t - 2] + e[t];
x[t] = z[t] + u[t];
// Change Points Create Rull
if (t == change_point_time * s) {
u[t] += u[0] - decrease_change_value;
u[0] -= decrease_change_value;
s++;
}
} else {
u[t] = u[t - 1];
e[t] = white_noise(expected_value, variance_value);
x[t] = e[t];
}
if (t == outlier_point) {
printf("% lf\n", x[t] + expected_change_total);
} else {
printf("% lf\n", x[t]);
}
t++;
}
free(x);
free(z);
free(a);
free(e);
free(u);
return 0;
MALLOC_ERROR:
fprintf(stderr, "Unable to malloc memory – fatal !\n");
exit(-1);
}
