// ---------------------------------------------------------------------------
// addNoiseEnergy
// ---------------------------------------------------------------------------
//! Add noise (bandwidth) energy to this Breakpoint by computing new
//! amplitude and bandwidth values. enoise may be negative, but
//! noise energy cannot be removed (negative energy added) in excess
//! of the current noise energy.
//!
//! \param enoise is the amount of noise energy to add to
//! this Breakpoint.
//
void
Breakpoint::addNoiseEnergy( double enoise )
{
// compute current energies:
double e = amplitude() * amplitude(); // current total energy
double n = e * bandwidth(); // current noise energy
// Assert( e >= n );
// could complain, but its recoverable, just fix it:
if ( e < n )
{
e = n;
}
// guard against divide-by-zero, and don't allow
// the sinusoidal energy to decrease:
if ( n + enoise > 0. )
{
// if new noise energy is positive, total
// energy must also be positive:
// Assert( e + enoise > 0 );
setBandwidth( (n + enoise) / (e + enoise) );
setAmplitude( std::sqrt(e + enoise) );
}
else
{
// if new noise energy is negative, leave
// all sinusoidal energy:
setBandwidth( 0. );
setAmplitude( std::sqrt( e - n ) );
}
}
/*
get the amplitude of a tree.
the amplitude of a path in a tree is the difference between the max and min in one path
from root to leaves. the amplitude of a tree is the max of amplitude of all the path.
*/
void amplitude(TreeNode* root, int maxNum, int minNum, int &res) {
if (!root) {
res = max(res, maxNum - minNum); // the end of one path.
return;
}
maxNum = max(maxNum, root->val);
minNum = min(minNum, root->val);
amplitude(root->left, maxNum, minNum, res);
amplitude(root->right, maxNum, minNum, res);
return;
}
int amplitude(TreeNode* root) {
int maxNum, minNum, res = 0;
if (!root) return 0;
maxNum = minNum = root->val;
amplitude(root, maxNum, minNum, res);
return res;
}
int main(int argc, char* argv[]) {
gflags::ParseCommandLineFlags(&argc, &argv, true);
std::shared_ptr<rgbd::DepthCamera> camera(
new rgbd::DS325(FLAGS_id, FRAME_FORMAT_VGA));
camera->start();
std::string execstr = "mkdir -p " + FLAGS_dir;
system(execstr.c_str());
cv::Mat color(camera->colorSize(), CV_8UC3);
cv::Mat amplitude(camera->depthSize(), CV_16UC1);
cv::namedWindow("color", CV_WINDOW_AUTOSIZE | CV_WINDOW_FREERATIO);
cv::namedWindow("depth", CV_WINDOW_AUTOSIZE | CV_WINDOW_FREERATIO);
while (cv::waitKey(10) != 0x1b) {
camera->captureColor(color);
camera->captureAmplitude(amplitude);
findChessboards(color, amplitude);
}
cv::destroyAllWindows();
return 0;
}
void EasingGraph::setAmplitude(qreal newAmplitude)
{
if ((amplitude() != newAmplitude) && ((easingShape()==QLatin1String("Bounce")) ||(easingShape()==QLatin1String("Elastic")))) {
m_curveFunction.setAmplitude(newAmplitude);
emit amplitudeChanged();
update();
}
}
int main(int argc, char *argv[])
{
QString ampFile("../example_data/amplitude_199x199_double.raw");
QString phaFile("../example_data/phase_199x199_double.raw");
QString solFile("../example_data/solution_199x199_double.raw");
QSize rawSize(199,199);
int sz = rawSize.width() * rawSize.height();
QFile amplitude(ampFile);
if (!amplitude.open(QIODevice::ReadOnly))
{
qDebug() << "amplitude file not found" << ampFile;
return -1;
}
double* amp = new double[sz];
amplitude.read((char*)amp, sz * sizeof(amp[0]));
amplitude.close();
QFile phase(phaFile);
if (!phase.open(QIODevice::ReadOnly))
{
qDebug() << "phase file not found" << phaFile;
return -1;
}
double* pha = new double[sz];
phase.read((char*)pha, sz * sizeof(pha[0]));
phase.close();
GSATracking up;
up.SetBranchCutMode(GSATracking::AMPLITUDETRACING); // aplitude tracking mode
up.SetCutsDilate(7); // dilatation kernel radius
up.SetInterpolate(true); // interpolation enabled
up.SetWeightedInterpolation(true); // amplitude based weighted interpolation
double* sol = up.UnWrapp(pha, amp, rawSize);
QFile solution(solFile);
solution.open(QIODevice::WriteOnly);
solution.write((char*)sol, sz * sizeof(sol[0]));
solution.close();
delete amp;
delete pha;
qDebug() << "positive residues:" << up.PositiveResidueCount();
qDebug() << "negative residues:" << up.NegativeResidueCount();
qDebug() << "solution file created:" << solFile;
qDebug() << endl << "press any key";
std::cin.get();
return 0;
}
Foam::tmp<Foam::scalarField> Foam::waveModels::solitary::elevation
(
const scalar t,
const scalar u,
const scalarField& x
) const
{
return amplitude(t)*Pi(t, u, x);
}
void amp(int T[], int N) {
int *A;
A = amplitude(T, N);
printf("The subset is:\n");
for(int ii = 0; ii < return_size; ii++) {
printf("%d: %d\n", ii, A[ii]);
}
}
void CVehicleMovementHelicopter::InitMovementNoise( const CVehicleParams& noiseParams, CVTolNoise& noise )
{
Vec3 amplitude(ZERO);
Vec3 frequency(ZERO);
Vec3 rotAmplitudeDeg(ZERO);
Vec3 rotFrequency(ZERO);
float startDamageRatio = 0.f;
noiseParams.getAttr("amplitude", amplitude);
noiseParams.getAttr("frequency", frequency);
noiseParams.getAttr("rotAmplitude", rotAmplitudeDeg);
noiseParams.getAttr("rotFrequency", rotFrequency);
noiseParams.getAttr("startDamageRatio", startDamageRatio);
noise.Init(amplitude, frequency, DEG2RAD(rotAmplitudeDeg), rotFrequency, startDamageRatio);
}
ComponentTransferFunction SVGComponentTransferFunctionElement::transferFunction() const
{
ComponentTransferFunction func;
func.type = (ComponentTransferType) type();
func.slope = slope();
func.intercept = intercept();
func.amplitude = amplitude();
func.exponent = exponent();
func.offset = offset();
SVGNumberList* numbers = tableValues();
ExceptionCode ec = 0;
unsigned int nr = numbers->numberOfItems();
for (unsigned int i = 0; i < nr; i++)
func.tableValues.append(numbers->getItem(i, ec));
return func;
}
void Base::step()
{
if (!m_active) return;
m_time += m_speed;
if (m_cycles > 0.0 && m_time > m_cycles) {
m_time = m_cycles;
m_active = false;
}
update(m_time, amplitude(m_time));
if (!m_active) {
finished(this);
finished();
}
}
Real32 calcPerlinNoise(const Pnt2f& t, Real32 Amplitude, Real32 Frequency, const Vec2f& Phase, Real32 Persistance, UInt32 Octaves, UInt32 UInt32, bool Smoothing)
{
Real32 total(0.0f), amplitude(Amplitude), frequency(Frequency);
Pnt2f pos(t + Phase);
for(unsigned int i(0); i < Octaves; ++i)
{
if(i > 0)
{
frequency *= 2;
amplitude *= Persistance;
}
total += interpolatedNoise(pos * frequency, i, UInt32, Smoothing) * amplitude;
}
return total;
}
/*!
Compare this easing curve with \a other and returns true if they are
equal. It will also compare the properties of a curve.
*/
bool QEasingCurve::operator==(const QEasingCurve &other) const
{
bool res = d_ptr->func == other.d_ptr->func
&& d_ptr->type == other.d_ptr->type;
if (res) {
if (d_ptr->config && other.d_ptr->config) {
// catch the config content
res = d_ptr->config->operator==(*(other.d_ptr->config));
} else if (d_ptr->config || other.d_ptr->config) {
// one one has a config object, which could contain default values
res = qFuzzyCompare(amplitude(), other.amplitude()) &&
qFuzzyCompare(period(), other.period()) &&
qFuzzyCompare(overshoot(), other.overshoot());
}
}
return res;
}
void scenarioModelSignalProcessing(std::default_random_engine &gen)
{
const int N = 500; //Signals modeling
const float delta_z = 1;
float E_max = 200; //Max amplitude
float sigma = 200;
dmod::array1d background(N, 100);
dmod::array1d amplitude(N, 50);
dmod::array1d frequency(N);
amplitude = dmod::sum(amplitude, dmod::fixedGaussianAmplitude(E_max, sigma, N/2, N));
for (int i = 0; i < N; i++)
{
frequency[i] = 0.105 - 0.00015*i;
}
//Estimated signal
dmod::array1d phase = dmod::phaseFromFrequency(frequency, 0, delta_z);
dmod::array1d noise = dmod::createNormalNoise(0, 10, N, gen);
dmod::array1d signal = dmod::createSignal1D(background, amplitude, phase, noise);
printer::print_signal("MatlabScripts/out.txt", signal);
printer::print_states("MatlabScripts/data.txt", background, amplitude, frequency, phase);
std::cout << dmod::snr(signal, noise) << std::endl;
//Creation of EKF
Eigen::Vector4d beginState(100, 40, 0.17985, 0);
Eigen::Matrix4d Rw;
Rw << 0.1, 0, 0, 0,
0, 0.15, 0, 0,
0, 0, 0.0005, 0,
0, 0, 0, 0.002;
float Rn = 0.5;
EKF filter(beginState, Eigen::Matrix4d::Identity(), Rw, Rn);
std::vector<Eigen::Vector4d> states = filter.estimateAll(signal);
dmod::array1d restoredSignal = filter.getRestoredSignal(states);
printer::print_states("MatlabScripts/EKF_data.txt", states);
printer::print_Kalman_stdev("MatlabScripts/EKF_deviations.txt", states, signal, noise, background, amplitude, frequency, phase, restoredSignal);
}
void Fresnel::spiral (DoubleVector &spiralX, DoubleVector &spiralY) const
{
spiralX.clear ();
spiralY.clear ();
spiralX.push_back (0.0);
spiralY.push_back (0.0);
Complex sp;
int fn = currentFresnelNumber;
double R = holeRadius;
double b = observerDistance;
double b2 = b * b;
double innerR = 0.0;
double outerR = 0.0;
double innerD = b;
double outerD = 0.0;
double dd = 0.0;
for (int n = 0; n < fn + 1; ++n) {
outerR = zoneOuterRadius (n);
outerD = sqrt (outerR*outerR + b2);
dd = (outerD - innerD) / accuracySpiral;
bool stop = false;
for (int i = 0; i < accuracySpiral && !stop; ++i) {
innerR = sqrt (innerD*innerD - b2);
innerD += dd;
outerR = sqrt (innerD*innerD - b2);
if (outerR >= R) {
outerR = R;
stop = true;
}
sp += amplitude (innerR, outerR);
spiralX.push_back (sp.im);
spiralY.push_back (sp.re);
}
}
}
OSG_BEGIN_NAMESPACE
Real32 calcPerlinNoise(Real32 t, Real32 Amplitude, Real32 Frequency, Real32 Phase, Real32 Persistance, UInt32 Octaves, UInt32 UInt32, bool Smoothing)
{
Real32 total(0.0f), amplitude(Amplitude), frequency(Frequency), pos(t + Phase);
for(unsigned int i(0); i < Octaves; ++i)
{
if(i > 0)
{
frequency *= 2.0f;
amplitude *= Persistance;
}
total += interpolatedNoise(pos * frequency, i, UInt32, Smoothing) * amplitude;
}
return total;
}
static autoIntensity Sound_to_Intensity_ (Sound me, double minimumPitch, double timeStep, int subtractMeanPressure) {
try {
/*
* Preconditions.
*/
if (! NUMdefined (minimumPitch)) Melder_throw (U"(Sound-to-Intensity:) Minimum pitch undefined.");
if (! NUMdefined (timeStep)) Melder_throw (U"(Sound-to-Intensity:) Time step undefined.");
if (timeStep < 0.0) Melder_throw (U"(Sound-to-Intensity:) Time step should be zero or positive instead of ", timeStep, U".");
if (my dx <= 0.0) Melder_throw (U"(Sound-to-Intensity:) The Sound's time step should be positive.");
if (minimumPitch <= 0.0) Melder_throw (U"(Sound-to-Intensity:) Minimum pitch should be positive.");
/*
* Defaults.
*/
if (timeStep == 0.0) timeStep = 0.8 / minimumPitch; // default: four times oversampling Hanning-wise
double windowDuration = 6.4 / minimumPitch;
Melder_assert (windowDuration > 0.0);
double halfWindowDuration = 0.5 * windowDuration;
long halfWindowSamples = (long) floor (halfWindowDuration / my dx);
autoNUMvector <double> amplitude (- halfWindowSamples, halfWindowSamples);
autoNUMvector <double> window (- halfWindowSamples, halfWindowSamples);
for (long i = - halfWindowSamples; i <= halfWindowSamples; i ++) {
double x = i * my dx / halfWindowDuration, root = 1 - x * x;
window [i] = root <= 0.0 ? 0.0 : NUMbessel_i0_f ((2 * NUMpi * NUMpi + 0.5) * sqrt (root));
}
long numberOfFrames;
double thyFirstTime;
try {
Sampled_shortTermAnalysis (me, windowDuration, timeStep, & numberOfFrames, & thyFirstTime);
} catch (MelderError) {
Melder_throw (U"The duration of the sound in an intensity analysis should be at least 6.4 divided by the minimum pitch (", minimumPitch, U" Hz), "
U"i.e. at least ", 6.4 / minimumPitch, U" s, instead of ", my xmax - my xmin, U" s.");
}
autoIntensity thee = Intensity_create (my xmin, my xmax, numberOfFrames, timeStep, thyFirstTime);
for (long iframe = 1; iframe <= numberOfFrames; iframe ++) {
double midTime = Sampled_indexToX (thee.peek(), iframe);
long midSample = Sampled_xToNearestIndex (me, midTime);
long leftSample = midSample - halfWindowSamples, rightSample = midSample + halfWindowSamples;
double sumxw = 0.0, sumw = 0.0, intensity;
if (leftSample < 1) leftSample = 1;
if (rightSample > my nx) rightSample = my nx;
for (long channel = 1; channel <= my ny; channel ++) {
for (long i = leftSample; i <= rightSample; i ++) {
amplitude [i - midSample] = my z [channel] [i];
}
if (subtractMeanPressure) {
double sum = 0.0;
for (long i = leftSample; i <= rightSample; i ++) {
sum += amplitude [i - midSample];
}
double mean = sum / (rightSample - leftSample + 1);
for (long i = leftSample; i <= rightSample; i ++) {
amplitude [i - midSample] -= mean;
}
}
for (long i = leftSample; i <= rightSample; i ++) {
sumxw += amplitude [i - midSample] * amplitude [i - midSample] * window [i - midSample];
sumw += window [i - midSample];
}
}
intensity = sumxw / sumw;
intensity /= 4e-10;
thy z [1] [iframe] = intensity < 1e-30 ? -300 : 10 * log10 (intensity);
}
return thee;
} catch (MelderError) {
Melder_throw (me, U": intensity analysis not performed.");
}
}
double SinusTrajectory::position(double time) const
{
return amplitude(time) * sin(2.0 * M_PI * frequency(time) * time + m_phase) + m_offset;
}
double Fresnel::intensity () const
{
Complex amp = amplitude ();
return amp.sqAbs () * 3e+11 / (8.0 * M_PI);
}
// CORE FUNCTION: decode a block of data
void block_decode(float *input1[2], float *input2[2], float *output[6], float center_width, float dimension, float adaption_rate) {
// 1. scale the input by the window function; this serves a dual purpose:
// - first it improves the FFT resolution b/c boundary discontinuities (and their frequencies) get removed
// - second it allows for smooth blending of varying filters between the blocks
{
float* pWnd = &wnd[0];
float* pLt = <[0];
float* pRt = &rt[0];
float* pIn0 = input1[0];
float* pIn1 = input1[1];
for (unsigned k=0;k<halfN;k++) {
*pLt++ = *pIn0++ * *pWnd;
*pRt++ = *pIn1++ * *pWnd++;
}
pIn0 = input2[0];
pIn1 = input2[1];
for (unsigned k=0;k<halfN;k++) {
*pLt++ = *pIn0++ * *pWnd;
*pRt++ = *pIn1++ * *pWnd++;
}
}
#ifdef USE_FFTW3
// ... and tranform it into the frequency domain
fftwf_execute(loadL);
fftwf_execute(loadR);
#else
ff_fft_permuteRC(fftContextForward, lt, (FFTComplex*)&dftL[0]);
av_fft_calc(fftContextForward, (FFTComplex*)&dftL[0]);
ff_fft_permuteRC(fftContextForward, rt, (FFTComplex*)&dftR[0]);
av_fft_calc(fftContextForward, (FFTComplex*)&dftR[0]);
#endif
// 2. compare amplitude and phase of each DFT bin and produce the X/Y coordinates in the sound field
// but dont do DC or N/2 component
for (unsigned f=0;f<halfN;f++) {
// get left/right amplitudes/phases
float ampL = amplitude(dftL[f]), ampR = amplitude(dftR[f]);
float phaseL = phase(dftL[f]), phaseR = phase(dftR[f]);
// if (ampL+ampR < epsilon)
// continue;
// calculate the amplitude/phase difference
float ampDiff = clamp((ampL+ampR < epsilon) ? 0 : (ampR-ampL) / (ampR+ampL));
float phaseDiff = phaseL - phaseR;
if (phaseDiff < -PI) phaseDiff += 2*PI;
if (phaseDiff > PI) phaseDiff -= 2*PI;
phaseDiff = abs(phaseDiff);
if (linear_steering) {
// --- this is the fancy new linear mode ---
// get sound field x/y position
yfs[f] = get_yfs(ampDiff,phaseDiff);
xfs[f] = get_xfs(ampDiff,yfs[f]);
// add dimension control
yfs[f] = clamp(yfs[f] - dimension);
// add crossfeed control
xfs[f] = clamp(xfs[f] * (front_separation*(1+yfs[f])/2 + rear_separation*(1-yfs[f])/2));
// 3. generate frequency filters for each output channel
float left = (1-xfs[f])/2, right = (1+xfs[f])/2;
float front = (1+yfs[f])/2, back = (1-yfs[f])/2;
float volume[5] = {
front * (left * center_width + max(0,-xfs[f]) * (1-center_width)), // left
front * center_level*((1-abs(xfs[f])) * (1-center_width)), // center
front * (right * center_width + max(0, xfs[f]) * (1-center_width)), // right
back * surround_level * left, // left surround
back * surround_level * right // right surround
};
// adapt the prior filter
for (unsigned c=0;c<5;c++)
filter[c][f] = (1-adaption_rate)*filter[c][f] + adaption_rate*volume[c];
} else {
// --- this is the old & simple steering mode ---
// calculate the amplitude/phase difference
float ampDiff = clamp((ampL+ampR < epsilon) ? 0 : (ampR-ampL) / (ampR+ampL));
float phaseDiff = phaseL - phaseR;
if (phaseDiff < -PI) phaseDiff += 2*PI;
if (phaseDiff > PI) phaseDiff -= 2*PI;
phaseDiff = abs(phaseDiff);
// determine sound field x-position
xfs[f] = ampDiff;
// determine preliminary sound field y-position from phase difference
yfs[f] = 1 - (phaseDiff/PI)*2;
if (abs(xfs[f]) > surround_balance) {
// blend linearly between the surrounds and the fronts if the balance exceeds the surround encoding balance
// this is necessary because the sound field is trapezoidal and will be stretched behind the listener
float frontness = (abs(xfs[f]) - surround_balance)/(1-surround_balance);
yfs[f] = (1-frontness) * yfs[f] + frontness * 1;
}
// add dimension control
yfs[f] = clamp(yfs[f] - dimension);
// add crossfeed control
xfs[f] = clamp(xfs[f] * (front_separation*(1+yfs[f])/2 + rear_separation*(1-yfs[f])/2));
// 3. generate frequency filters for each output channel, according to the signal position
// the sum of all channel volumes must be 1.0
float left = (1-xfs[f])/2, right = (1+xfs[f])/2;
float front = (1+yfs[f])/2, back = (1-yfs[f])/2;
float volume[5] = {
front * (left * center_width + max(0,-xfs[f]) * (1-center_width)), // left
front * center_level*((1-abs(xfs[f])) * (1-center_width)), // center
front * (right * center_width + max(0, xfs[f]) * (1-center_width)), // right
back * surround_level*max(0,min(1,((1-(xfs[f]/surround_balance))/2))), // left surround
back * surround_level*max(0,min(1,((1+(xfs[f]/surround_balance))/2))) // right surround
};
// adapt the prior filter
for (unsigned c=0;c<5;c++)
filter[c][f] = (1-adaption_rate)*filter[c][f] + adaption_rate*volume[c];
}
// ... and build the signal which we want to position
frontL[f] = polar(ampL+ampR,phaseL);
frontR[f] = polar(ampL+ampR,phaseR);
avg[f] = frontL[f] + frontR[f];
surL[f] = polar(ampL+ampR,phaseL+phase_offsetL);
surR[f] = polar(ampL+ampR,phaseR+phase_offsetR);
trueavg[f] = cfloat(dftL[f][0] + dftR[f][0], dftL[f][1] + dftR[f][1]);
}
// 4. distribute the unfiltered reference signals over the channels
apply_filter(&frontL[0],&filter[0][0],&output[0][0]); // front left
apply_filter(&avg[0], &filter[1][0],&output[1][0]); // front center
apply_filter(&frontR[0],&filter[2][0],&output[2][0]); // front right
apply_filter(&surL[0],&filter[3][0],&output[3][0]); // surround left
apply_filter(&surR[0],&filter[4][0],&output[4][0]); // surround right
apply_filter(&trueavg[0],&filter[5][0],&output[5][0]); // lfe
}
int main( )
{
USING_NAMESPACE_ACADO
// INTRODUCE THE VARIABLES:
// -------------------------
DifferentialState xB;
DifferentialState xW;
DifferentialState vB;
DifferentialState vW;
Control F;
// DEFINE A DIFFERENTIAL EQUATION:
// -------------------------------
double h = 0.05;
DiscretizedDifferentialEquation f( h );
// f << next(xB) == ( 9.523918456856767e-01*xB - 3.093442425036754e-03*xW + 4.450257887258270e-04*vW - 2.380407715716160e-07*F );
// f << next(xW) == ( -1.780103154903307e+00*xB - 1.005721624707961e+00*xW - 3.093442425036752e-03*vW - 8.900515774516536e-06*F );
// f << next(vB) == ( -5.536210379145256e+00*xB - 2.021981836435758e-01*xW + 1.0*vB + 2.474992857984263e-02*vW + 1.294618052471308e-04*F );
// f << next(vW) == ( 1.237376970014700e+01*xB + 1.183104351525840e+01*xW - 1.005721624707961e+00*vW + 6.186884850073496e-05*F );
f << next(xB) == ( 0.9335*xB + 0.0252*xW + 0.048860*vB + 0.000677*vW + 3.324e-06*F );
f << next(xW) == ( 0.1764*xB - 0.9821*xW + 0.004739*vB - 0.002591*vW - 8.822e-06*F );
f << next(vB) == ( -2.5210*xB - 0.1867*xW + 0.933500*vB + 0.025200*vW + 0.0001261*F );
f << next(vW) == ( -1.3070*xB + 11.670*xW + 0.176400*vB - 0.982100*vW + 6.536e-05*F );
OutputFcn g;
g << xB;
g << 500.0*vB + F;
DynamicSystem dynSys( f,g );
// SETUP THE PROCESS:
// ------------------
Vector mean( 1 ), amplitude( 1 );
mean.setZero( );
amplitude.setAll( 50.0 );
GaussianNoise myNoise( mean,amplitude );
Actuator myActuator( 1 );
myActuator.setControlNoise( myNoise,0.1 );
myActuator.setControlDeadTimes( 0.1 );
mean.setZero( );
amplitude.setAll( 0.001 );
UniformNoise myOutputNoise1( mean,amplitude );
mean.setAll( 20.0 );
amplitude.setAll( 10.0 );
GaussianNoise myOutputNoise2( mean,amplitude );
Sensor mySensor( 2 );
mySensor.setOutputNoise( 0,myOutputNoise1,0.1 );
mySensor.setOutputNoise( 1,myOutputNoise2,0.1 );
mySensor.setOutputDeadTimes( 0.15 );
Process myProcess;
myProcess.setDynamicSystem( dynSys );
myProcess.set( ABSOLUTE_TOLERANCE,1.0e-8 );
myProcess.setActuator( myActuator );
myProcess.setSensor( mySensor );
Vector x0( 4 );
x0.setZero( );
x0( 0 ) = 0.01;
myProcess.initializeStartValues( x0 );
myProcess.set( PLOT_RESOLUTION,HIGH );
// myProcess.set( CONTROL_PLOTTING,PLOT_NOMINAL );
// myProcess.set( PARAMETER_PLOTTING,PLOT_NOMINAL );
myProcess.set( OUTPUT_PLOTTING,PLOT_REAL );
GnuplotWindow window;
window.addSubplot( xB, "Body Position [m]" );
window.addSubplot( xW, "Wheel Position [m]" );
window.addSubplot( vB, "Body Velocity [m/s]" );
window.addSubplot( vW, "Wheel Velocity [m/s]" );
window.addSubplot( F,"Damping Force [N]" );
window.addSubplot( g(0),"Output 1" );
window.addSubplot( g(1),"Output 2" );
myProcess << window;
// SIMULATE AND GET THE RESULTS:
// -----------------------------
VariablesGrid u( 1,0.0,1.0,6 );
u( 0,0 ) = 10.0;
u( 1,0 ) = -200.0;
u( 2,0 ) = 200.0;
u( 3,0 ) = 200.0;
u( 4,0 ) = 0.0;
u( 5,0 ) = 0.0;
myProcess.init( 0.0,x0,u.getFirstVector() );
myProcess.run( u );
VariablesGrid uNom, uSim, ySim, ySens, xSim;
// myProcess.getLast( LOG_NOMINAL_CONTROLS,uNom ); uNom.print( "uNom" );
// myProcess.getLast( LOG_SIMULATED_CONTROLS,uSim ); uSim.print( "uSim" );
// myProcess.getLast( LOG_SIMULATED_OUTPUT,ySim ); ySim.print( "ySim" );
// myProcess.getLast( LOG_PROCESS_OUTPUT,ySens ); ySens.print( "ySens" );
//
// myProcess.getLast( LOG_DIFFERENTIAL_STATES,xSim );
return 0;
}
/*! \brief Process one frame of data with avmd algorithm
* @author Eric des Courtis
* @param session An avmd session
* @param frame A audio frame
*/
static void avmd_process(avmd_session_t *session, switch_frame_t *frame)
{
switch_event_t *event;
switch_status_t status;
switch_event_t *event_copy;
switch_channel_t *channel;
circ_buffer_t *b;
size_t pos;
double f;
double a;
double error = 0.0;
double success = 0.0;
double amp = 0.0;
double s_rate;
double e_rate;
double avg_a;
double sine_len;
uint32_t sine_len_i;
int valid;
b = &session->b;
/*! If beep has already been detected skip the CPU heavy stuff */
if(session->state.beep_state == BEEP_DETECTED){
return;
}
/*! Precompute values used heavily in the inner loop */
sine_len_i = SINE_LEN(session->rate);
sine_len = (double)sine_len_i;
channel = switch_core_session_get_channel(session->session);
/*! Insert frame of 16 bit samples into buffer */
INSERT_INT16_FRAME(b, (int16_t *)(frame->data), frame->samples);
/*! INNER LOOP -- OPTIMIZATION TARGET */
for(pos = GET_BACKLOG_POS(b); pos != (GET_CURRENT_POS(b) - P); pos++){
/*! Get a desa2 frequency estimate in Hertz */
f = TO_HZ(session->rate, desa2(b, pos));
/*! Don't caculate amplitude if frequency is not within range */
if(f < MIN_FREQUENCY || f > MAX_FREQUENCY) {
a = 0.0;
error += 1.0;
} else {
a = amplitude(b, pos, f);
success += 1.0;
if(!ISNAN(a)){
amp += a;
}
}
/*! Every once in a while we evaluate the desa2 and amplitude results */
if(((pos + 1) % sine_len_i) == 0){
s_rate = success / (error + success);
e_rate = error / (error + success);
avg_a = amp / sine_len;
/*! Results out of these ranges are considered invalid */
valid = 0;
if( s_rate > 0.60 && avg_a > 0.50) valid = 1;
else if(s_rate > 0.65 && avg_a > 0.45) valid = 1;
else if(s_rate > 0.70 && avg_a > 0.40) valid = 1;
else if(s_rate > 0.80 && avg_a > 0.30) valid = 1;
else if(s_rate > 0.95 && avg_a > 0.05) valid = 1;
else if(s_rate >= 0.99 && avg_a > 0.04) valid = 1;
else if(s_rate == 1.00 && avg_a > 0.02) valid = 1;
if(valid) {
APPEND_SMA_VAL(&session->sma_b, s_rate * avg_a);
}
else {
APPEND_SMA_VAL(&session->sma_b, 0.0 );
}
/*! If sma is higher then 0 we have some kind of detection (increase this value to eliminate false positives ex: 0.01) */
if(session->sma_b.sma > 0.00){
/*! Throw an event to FreeSWITCH */
status = switch_event_create_subclass(&event, SWITCH_EVENT_CUSTOM, AVMD_EVENT_BEEP);
if(status != SWITCH_STATUS_SUCCESS) {
return;
}
switch_event_add_header_string(event, SWITCH_STACK_BOTTOM, "Beep-Status", "stop");
switch_event_add_header_string(event, SWITCH_STACK_BOTTOM, "Unique-ID", switch_core_session_get_uuid(session->session));
switch_event_add_header_string(event, SWITCH_STACK_BOTTOM, "call-command", "avmd");
if ((switch_event_dup(&event_copy, event)) != SWITCH_STATUS_SUCCESS) {
return;
}
switch_core_session_queue_event(session->session, &event);
switch_event_fire(&event_copy);
switch_log_printf(SWITCH_CHANNEL_SESSION_LOG(session->session), SWITCH_LOG_INFO, "<<< AVMD - Beep Detected >>>\n");
switch_channel_set_variable(channel, "avmd_detect", "TRUE");
RESET_SMA_BUFFER(&session->sma_b);
session->state.beep_state = BEEP_DETECTED;
return;
}
amp = 0.0;
success = 0.0;
error = 0.0;
}
}
}
Foam::scalar Foam::waveModels::solitary::k(const scalar t) const
{
return sqrt(0.75*amplitude(t)/pow3(depth()));
}
Foam::scalar Foam::waveModels::solitary::alpha(const scalar t) const
{
return amplitude(t)/depth();
}