Paparazzi UAS v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
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
17static void r250_init(int seed);
18//static unsigned int r250( void );
19static 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
28static long set_seed(long);
29//static long get_seed(void);
30static unsigned long int randlcg(void);
31#endif
32
33
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
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
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
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
65void double_vect3_update_random_walk(struct DoubleVect3 *rw, struct DoubleVect3 *std_dev, double dt, double thau)
66{
67 struct DoubleVect3 drw;
69 struct DoubleVect3 tmp;
70 VECT3_SMUL(tmp, *rw, (-1. / thau));
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*/
84double 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>
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
163static unsigned int r250_buffer[ 250 ];
164static int r250_index;
165
166static 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 */
197static 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
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 */
223static 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
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
254static long int quotient = LONG_MAX / 16807L;
255static long int my_remainder = LONG_MAX % 16807L;
256
257static long int seed_val = 1L;
258
259static 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 */
269unsigned 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 {
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)
#define VECT3_ADD(_a, _b)
uint16_t foo
Definition main_demo5.c:58
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