fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

white noise generator updated

Browse Source
This commit is contained in:
romain 2019-03-12 14:11:15 -04:00 committed by Beat Küng
parent cdd7e95a92
commit a9660d9da0
1 changed files with 25 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

28
src/modules/sih/sih.cpp
View File

@ -637,14 +637,36 @@ void Sih::send_serial_msg(int serial_fd, int64_t t_ms)
float Sih::generate_wgn() // generate white Gaussian noise sample with std=1
{
float temp=((float)(rand()+1))/(((float)RAND_MAX+1.0f));
// algorithm 1:
// float temp=((float)(rand()+1))/(((float)RAND_MAX+1.0f));
// return sqrtf(-2.0f*logf(temp))*cosf(2.0f*M_PI_F*rand()/RAND_MAX);
// algorithm 2: from BlockRandGauss.hpp
static float V1, V2, S;
static bool phase = true;
float X;
return sqrtf(-2.0f*logf(temp))*cosf(2.0f*M_PI_F*rand()/RAND_MAX);
if (phase) {
do {
float U1 = (float)rand() / RAND_MAX;
float U2 = (float)rand() / RAND_MAX;
V1 = 2.0f * U1 - 1.0f;
V2 = 2.0f * U2 - 1.0f;
S = V1 * V1 + V2 * V2;
} while (S >= 1.0f || fabsf(S) < 1e-8f);
X = V1 * float(sqrtf(-2.0f * float(logf(S)) / S));
} else {
X = V2 * float(sqrtf(-2.0f * float(logf(S)) / S));
}
phase = !phase;
return X;
}
Vector3f Sih::noiseGauss3f(float stdx,float stdy, float stdz) // generate white Gaussian noise sample with specified std
{
return Vector3f(generate_wgn()*stdx,generate_wgn()*stdy,generate_wgn()*stdz);
return Vector3f(generate_wgn()*stdx,generate_wgn()*stdy,generate_wgn()*stdz);
} // there is another wgn algorithm in BlockRandGauss.hpp
int sih_main(int argc, char *argv[])