From a9660d9da06d3e278a45a57bd39abe4eceb7ac7b Mon Sep 17 00:00:00 2001
From: romain <romain.chiap@gmail.com>
Date: Tue, 12 Mar 2019 14:11:15 -0400
Subject: [PATCH] white noise generator updated
---
src/modules/sih/sih.cpp | 28 +++++++++++++++++++++++++---
1 file changed, 25 insertions(+), 3 deletions(-)
diff --git a/src/modules/sih/sih.cpp b/src/modules/sih/sih.cpp
index 31ff81b720..d4255ffbdf 100644
--- a/src/modules/sih/sih.cpp
+++ b/src/modules/sih/sih.cpp
@@ -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;
+
+ 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));
- return sqrtf(-2.0f*logf(temp))*cosf(2.0f*M_PI_F*rand()/RAND_MAX);
+ } 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[])
--
GitLab