Paparazzi UAS  v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
nps_random.c
Go to the documentation of this file.
1 #include "nps_random.h"
2 
3 
4 #include <math.h>
5 #include <limits.h>
6 
7 
8 #if 0
9 /*
10  * R250
11  * Kirkpatrick, S., and E. Stoll, 1981; "A Very Fast
12  * Shift-Register Sequence Random Number Generator",
13  * Journal of Computational Physics, V.40
14  *
15  */
16 
17 static void r250_init(int seed);
18 //static unsigned int r250( void );
19 static double dr250(void);
20 
21 /*
22  * randclg
23  * Linear Congruential Method, the "minimal standard generator"
24  * Park & Miller, 1988, Comm of the ACM, 31(10), pp. 1192-1201
25  *
26  */
27 
28 static long set_seed(long);
29 //static long get_seed(void);
30 static unsigned long int randlcg(void);
31 #endif
32 
33 
34 void double_vect3_add_gaussian_noise(struct DoubleVect3 *vect, struct DoubleVect3 *std_dev)
35 {
36  vect->x += get_gaussian_noise() * std_dev->x;
37  vect->y += get_gaussian_noise() * std_dev->y;
38  vect->z += get_gaussian_noise() * std_dev->z;
39 }
40 
41 void float_vect3_add_gaussian_noise(struct FloatVect3 *vect, struct FloatVect3 *std_dev)
42 {
43  vect->x += get_gaussian_noise() * std_dev->x;
44  vect->y += get_gaussian_noise() * std_dev->y;
45  vect->z += get_gaussian_noise() * std_dev->z;
46 }
47 
48 void float_rates_add_gaussian_noise(struct FloatRates *vect, struct FloatRates *std_dev)
49 {
50  vect->p += get_gaussian_noise() * std_dev->p;
51  vect->q += get_gaussian_noise() * std_dev->q;
52  vect->r += get_gaussian_noise() * std_dev->r;
53 }
54 
55 
56 
57 void double_vect3_get_gaussian_noise(struct DoubleVect3 *vect, struct DoubleVect3 *std_dev)
58 {
59  vect->x = get_gaussian_noise() * std_dev->x;
60  vect->y = get_gaussian_noise() * std_dev->y;
61  vect->z = get_gaussian_noise() * std_dev->z;
62 }
63 
64 
65 void double_vect3_update_random_walk(struct DoubleVect3 *rw, struct DoubleVect3 *std_dev, double dt, double thau)
66 {
67  struct DoubleVect3 drw;
68  double_vect3_get_gaussian_noise(&drw, std_dev);
69  struct DoubleVect3 tmp;
70  VECT3_SMUL(tmp, *rw, (-1. / thau));
71  VECT3_ADD(drw, tmp);
72  VECT3_SMUL(drw, drw, dt);
73  VECT3_ADD(*rw, drw);
74 }
75 
76 
77 
78 
79 
80 #if 0
81 /*
82  http://www.taygeta.com/random/gaussian.html
83 */
84 double get_gaussian_noise(void)
85 {
86 
87  double x1;
88  static int nb_call = 0;
89  static double x2, w;
90  if (nb_call == 0) { r250_init(0); }
91  nb_call++;
92  if (nb_call % 2) {
93  do {
94  x1 = 2.0 * dr250() - 1.0;
95  x2 = 2.0 * dr250() - 1.0;
96  w = x1 * x1 + x2 * x2;
97  } while (w >= 1.0);
98 
99  w = sqrt((-2.0 * log(w)) / w);
100  return x1 * w;
101  } else {
102  return x2 * w;
103  }
104 }
105 #else
106 #include <gsl/gsl_rng.h>
107 #include <gsl/gsl_randist.h>
108 #include <stdlib.h>
109 double get_gaussian_noise(void)
110 {
111  static gsl_rng *r = NULL;
112  // select random number generator
113  if (!r) { r = gsl_rng_alloc(gsl_rng_mt19937); }
114  return gsl_ran_gaussian(r, 1.);
115 }
116 #endif
117 
118 
119 #if 0
120 /*
121  * R250
122  * Kirkpatrick, S., and E. Stoll, 1981; "A Very Fast
123  * Shift-Register Sequence Random Number Generator",
124  * Journal of Computational Physics, V.40
125  *
126  */
127 
128 /* defines to allow for 16 or 32 bit integers */
129 #define BITS 31
130 
131 
132 #if WORD_BIT == 32
133 #ifndef BITS
134 #define BITS 32
135 #endif
136 #else
137 #ifndef BITS
138 #define BITS 16
139 #endif
140 #endif
141 
142 #if BITS == 31
143 #define MSB 0x40000000L
144 #define ALL_BITS 0x7fffffffL
145 #define HALF_RANGE 0x20000000L
146 #define STEP 7
147 #endif
148 
149 #if BITS == 32
150 #define MSB 0x80000000L
151 #define ALL_BITS 0xffffffffL
152 #define HALF_RANGE 0x40000000L
153 #define STEP 7
154 #endif
155 
156 #if BITS == 16
157 #define MSB 0x8000
158 #define ALL_BITS 0xffff
159 #define HALF_RANGE 0x4000
160 #define STEP 11
161 #endif
162 
163 static unsigned int r250_buffer[ 250 ];
164 static int r250_index;
165 
166 static void r250_init(int sd)
167 {
168  int j, k;
169  unsigned int mask, msb;
170  set_seed(sd);
171 
172  r250_index = 0;
173  for (j = 0; j < 250; j++) { /* fill r250 buffer with BITS-1 bit values */
174  r250_buffer[j] = randlcg();
175  }
176 
177  for (j = 0; j < 250; j++) /* set some MSBs to 1 */
178  if (randlcg() > HALF_RANGE) {
179  r250_buffer[j] |= MSB;
180  }
181 
182  msb = MSB; /* turn on diagonal bit */
183  mask = ALL_BITS; /* turn off the leftmost bits */
184 
185  for (j = 0; j < BITS; j++) {
186  k = STEP * j + 3; /* select a word to operate on */
187  r250_buffer[k] &= mask; /* turn off bits left of the diagonal */
188  r250_buffer[k] |= msb; /* turn on the diagonal bit */
189  mask >>= 1;
190  msb >>= 1;
191  }
192 
193 }
194 
195 #if 0
196 /* returns a random unsigned integer */
197 static unsigned int r250(void)
198 {
199  register int j;
200  register unsigned int new_rand;
201 
202  if (r250_index >= 147) {
203  j = r250_index - 147; /* wrap pointer around */
204  } else {
205  j = r250_index + 103;
206  }
207 
208  new_rand = r250_buffer[ r250_index ] ^ r250_buffer[ j ];
209  r250_buffer[ r250_index ] = new_rand;
210 
211  if (r250_index >= 249) { /* increment pointer for next time */
212  r250_index = 0;
213  } else {
214  r250_index++;
215  }
216 
217  return new_rand;
218 
219 }
220 #endif
221 
222 /* returns a random double in range 0..1 */
223 static double dr250(void)
224 {
225  register int j;
226  register unsigned int new_rand;
227 
228  if (r250_index >= 147) {
229  j = r250_index - 147; /* wrap pointer around */
230  } else {
231  j = r250_index + 103;
232  }
233 
234  new_rand = r250_buffer[ r250_index ] ^ r250_buffer[ j ];
235  r250_buffer[ r250_index ] = new_rand;
236 
237  if (r250_index >= 249) { /* increment pointer for next time */
238  r250_index = 0;
239  } else {
240  r250_index++;
241  }
242 
243  return (double)new_rand / ALL_BITS;
244 
245 }
246 
247 /*
248  * randclg
249  * Linear Congruential Method, the "minimal standard generator"
250  * Park & Miller, 1988, Comm of the ACM, 31(10), pp. 1192-1201
251  *
252  */
253 
254 static long int quotient = LONG_MAX / 16807L;
255 static long int my_remainder = LONG_MAX % 16807L;
256 
257 static long int seed_val = 1L;
258 
259 static long set_seed(long int sd)
260 {
261  return seed_val = sd;
262 }
263 
264 //static long get_seed(void) {
265 // return seed_val;
266 //}
267 
268 /* returns a random unsigned integer */
269 unsigned long int randlcg()
270 {
271  if (seed_val <= quotient) {
272  seed_val = (seed_val * 16807L) % LONG_MAX;
273  } else {
274  long int high_part = seed_val / quotient;
275  long int low_part = seed_val % quotient;
276 
277  long int test = 16807L * low_part - my_remainder * high_part;
278 
279  if (test > 0) {
280  seed_val = test;
281  } else {
282  seed_val = test + LONG_MAX;
283  }
284 
285  }
286 
287  return seed_val;
288 }
289 
290 #endif /* 0*/
float q
in rad/s
float p
in rad/s
float r
in rad/s
angular rates
#define VECT3_SMUL(_vo, _vi, _s)
Definition: pprz_algebra.h:189
#define VECT3_ADD(_a, _b)
Definition: pprz_algebra.h:147
void double_vect3_get_gaussian_noise(struct DoubleVect3 *vect, struct DoubleVect3 *std_dev)
Definition: nps_random.c:57
void double_vect3_add_gaussian_noise(struct DoubleVect3 *vect, struct DoubleVect3 *std_dev)
Definition: nps_random.c:34
void double_vect3_update_random_walk(struct DoubleVect3 *rw, struct DoubleVect3 *std_dev, double dt, double thau)
Definition: nps_random.c:65
void float_vect3_add_gaussian_noise(struct FloatVect3 *vect, struct FloatVect3 *std_dev)
Definition: nps_random.c:41
void float_rates_add_gaussian_noise(struct FloatRates *vect, struct FloatRates *std_dev)
Definition: nps_random.c:48
double get_gaussian_noise(void)
Definition: nps_random.c:109