// -----------------------------------------------------------------------------
//
// -----------------------------------------------------------------------------
void AddBadData::execute()
{
setErrorCondition(0);
dataCheck();
if(getErrorCondition() < 0) { return; }
add_noise();
// If there is an error set this to something negative and also set a message
notifyStatusMessage(getHumanLabel(), "Complete");
}
int main(int argc, char* argv[])
{
if(argc<3)
{
help();
}
else
{
add_noise(argc,argv);
}
std::cout << "Press any key to exit..." << std::endl;
std::cin.get();
return 0;
}
/*
update the helicopter simulation by one time step
*/
void Helicopter::update(const struct sitl_input &input)
{
float swash1 = (input.servos[0]-1000) / 1000.0f;
float swash2 = (input.servos[1]-1000) / 1000.0f;
float swash3 = (input.servos[2]-1000) / 1000.0f;
float tail_rotor = (input.servos[3]-1000) / 1000.0f;
float rsc = (input.servos[7]-1000) / 1000.0f;
// how much time has passed?
float delta_time = frame_time_us * 1.0e-6f;
float thrust = (rsc/rsc_setpoint)*(swash1+swash2+swash3)/3.0f;
// very simplistic mapping to body euler rates
float roll_rate = swash1 - swash2;
float pitch_rate = (swash1 + swash2)/2.0f - swash3;
float yaw_rate = tail_rotor - 0.5f;
float rsc_scale = rsc/rsc_setpoint;
roll_rate *= rsc_scale;
pitch_rate *= rsc_scale;
yaw_rate *= rsc_scale;
// rotational acceleration, in rad/s/s, in body frame
Vector3f rot_accel;
rot_accel.x = roll_rate * roll_rate_max;
rot_accel.y = pitch_rate * pitch_rate_max;
rot_accel.z = yaw_rate * yaw_rate_max;
// rotational air resistance
rot_accel.x -= gyro.x * radians(5000.0) / terminal_rotation_rate;
rot_accel.y -= gyro.y * radians(5000.0) / terminal_rotation_rate;
rot_accel.z -= gyro.z * radians(400.0) / terminal_rotation_rate;
// torque effect on tail
rot_accel.z += (rsc_scale+thrust) * rotor_rot_accel;
// update rotational rates in body frame
gyro += rot_accel * delta_time;
// update attitude
dcm.rotate(gyro * delta_time);
dcm.normalize();
// air resistance
Vector3f air_resistance = -velocity_ef * (GRAVITY_MSS/terminal_velocity);
// scale thrust to newtons
thrust *= thrust_scale;
accel_body = Vector3f(0, yaw_rate * rsc_scale * tail_thrust_scale, -thrust / mass);
Vector3f accel_earth = dcm * accel_body;
accel_earth += Vector3f(0, 0, GRAVITY_MSS);
accel_earth += air_resistance;
// if we're on the ground, then our vertical acceleration is limited
// to zero. This effectively adds the force of the ground on the aircraft
if (on_ground(position) && accel_earth.z > 0) {
accel_earth.z = 0;
}
// work out acceleration as seen by the accelerometers. It sees the kinematic
// acceleration (ie. real movement), plus gravity
accel_body = dcm.transposed() * (accel_earth + Vector3f(0, 0, -GRAVITY_MSS));
// add some noise
add_noise(thrust / thrust_scale);
// new velocity vector
velocity_ef += accel_earth * delta_time;
// new position vector
Vector3f old_position = position;
position += velocity_ef * delta_time;
// assume zero wind for now
airspeed = velocity_ef.length();
// constrain height to the ground
if (on_ground(position)) {
if (!on_ground(old_position)) {
printf("Hit ground at %f m/s\n", velocity_ef.z);
velocity_ef.zero();
// zero roll/pitch, but keep yaw
float r, p, y;
dcm.to_euler(&r, &p, &y);
dcm.from_euler(0, 0, y);
position.z = -(ground_level + frame_height - home.alt*0.01f);
}
}
// update lat/lon/altitude
update_position();
}
void comparacionQuantaVis (char *nombreFunc, char *nombreMalla) {
FuncionBayesVoronoiCBI *func = new FuncionBayesVoronoiCBI (nombreFunc);
funcL *fL = func->getFL();
double quanta = 0, ret, quanta_ori;
struct uvf_sample **samples_ori; // Arreglo con las visibilidades
Inicializador *ini = new InicializadorArchivo(nombreMalla);
ReconstructorGC *r;
std::ofstream archivo ("L_vs_quanta.dat");
archivo.close();
archivo.open ("L_vs_noise_vis.dat");
archivo.close();
archivo.open ("L_vs_N.dat");
archivo.close();
archivo.open ("chi2_vs_quanta.dat");
archivo.close();
archivo.open ("chi2_vs_noise_vis.dat");
archivo.close();
archivo.open ("chi2_vs_N.dat");
archivo.close();
archivo.open ("S_vs_quanta.dat");
archivo.close();
archivo.open ("S_vs_noise_vis.dat");
archivo.close();
archivo.open ("S_vs_N.dat");
archivo.close();
archivo.open ("dchi2_vs_quanta.dat");
archivo.close();
archivo.open ("dchi2_vs_noise_vis.dat");
archivo.close();
archivo.open ("dchi2_vs_N.dat");
archivo.close();
archivo.open ("dS_vs_quanta.dat");
archivo.close();
archivo.open ("dS_vs_noise_vis.dat");
archivo.close();
archivo.open ("dS_vs_N.dat");
archivo.close();
archivo.open ("quanta_vs_noise_vis.dat");
archivo.close();
double *pars = new double [func->getNPars()];
struct image *im_ori = do_read (fL->nombreFits);
samples_ori = (struct uvf_sample **) malloc(fL->n_archivos *
sizeof(struct uvf_sample *));
quanta_ori = func->getRuido();
for(int i = 0; i < fL->n_archivos; i++) {
// abrimos los archivos de visibilidades
int status = do_read_uvdata(fL->infile[i], &(fL->header_obs[i]),
&(samples_ori[i]));
if (status != SUCCESS) {
printf("Error con el archivo uvf\n");
exit(1);
}
}
do_write_fits (im_ori, "!fLfgImage.fits");
for (int arch = 0; arch < fL->n_archivos; arch++) {
mockcbi_sub (fL->cmb_image, im_ori,
fL->header_obs[arch], fL->samples_obs[arch], samples_ori[arch],
fL->beam);
}
long seed = 0;
int cont = 0;
for (double noiseScale = 1e-2; noiseScale < 1; noiseScale += 1e-2) {
cont++;
r = new ReconstructorGC (func, ini, 1e-10);
copy_vis (fL->header_obs, samples_ori, fL->samples_mod, fL->n_archivos);
noise_scale (fL->header_obs, fL->samples_mod, fL->n_archivos, noiseScale);
add_noise (fL->header_obs, fL->samples_mod, fL->n_archivos, &seed);
quanta = Irms (fL->header_obs, fL->samples_mod, fL->n_archivos);
func->setRuido (quanta);
for (int i = 0; i < func->getNPars() / 3; i++) {
r->setPar (3 * i + 2, r->getPar (3 * i + 2) / quanta_ori);
}
ret = r->run ();
for (int i = 0; i < func->getNPars(); i++) {
pars[i] = r->getPar(i);
}
guardar (func, pars, quanta, noiseScale);
func->guardarInfo ("Quanta" + i2s (cont, 5));
delete r;
}
}
int main(int argc, char **argv) {
struct image *im = do_read (argv[1]);
struct uvf_header *header [1];
struct uvf_sample *samples [1];
funcL *fL;
long seed;
//resize_map(im , 128, 128, 32, 32);
//do_write_fits (im, argv[2]);
//exit (0);
do_read_uvdata(argv[2], &(header[0]), &(samples[0]));
struct pbeam beam; // Beam del CBI
init_beam (&(beam));
beam.type = CBI;
double ***aten = atenuacion(im, header, 1, beam);
dirtyMap (im, header, samples, 1, aten);
do_write_fits (im, "!dirtyMap.fits");
fL = newFuncL (argv[1], argv + 2, argc - 4, 150, 0, 0, 500.0, 500.0, -1);
mockcbi_sub (fL->cmb_image, fL->fg_image,
fL->header_obs[0], fL->samples_obs[0], fL->samples_mod[0],
fL->beam);
seed = -labs((long)time(0));
add_noise (fL->header_obs[0],
fL->samples_mod[0], &seed);
do_write ("write uvf", argv[4], fL->header_obs[0],
fL->samples_mod[0], argv[2]);
//imagen2ascii (argv[1], argv[2]);
/*
int n = 150, cx, cy, arch, chan, samp, i;
float ftol = 1e-20;
funcL *fL;
struct image *punt = do_read (argv[1]);
double ascale;
FILE *archivo = fopen ("correlacion.dat", "w");
FILE *output = fopen ("crear_imagen_out.dat", "w");
struct image *imagen_in = do_read (argv[1]);
//struct image *imagen_out = do_read (argv[2]);
long seed;
printf ("%s, %s, %s, %s\n\n", argv[1], argv[2], argv[3], argv[4]);
fprintf (output, "crear_imagen %s, %s, %s, %s\n\n",
argv[1], argv[2], argv[3], argv[4]);
fL = newFuncL (argv[1], argv + 2, argc - 4, n, 0, 0);
//do_write_fits (fL->fg_image, argv[3]);
copy_empty_map (punt, fL->fg_image);
delete_map(fL->fg_image);
//for (i = 0; i < MAXDIM; i++) {
//punt->cdelt[i] *= 1.5;
//fL->cmb_image->cdelt[i] *= 1.5;
//}
fL->fg_image = punt;
//punt->pixels[64 + 64 * fL->fg_image->size[0]] = 0.1;
insertarGaussiana (30, 30, 10, punt, 50);
//insertarGaussiana (25, 27, 5, punt, 1.5);
//insertarGaussiana (40, 40, 3, punt, 1);
insertarGaussiana (45, 45, 4, punt, 10);
insertarGaussiana (32, 15, 4, punt, 10);
insertarRectangulo (32, 27, 50, 37, punt, 0.3);
escalarImagenRuido (punt, fL->header_obs, fL->samples_obs, fL->n_archivos, 100);
punt->datamin = 1e10;
punt->datamax = -1e10;
for (i = 0; i < punt->npixels; i++) {
if (punt->pixels[i] < punt->datamin) {
punt->datamin = punt->pixels[i];
}
if (punt->pixels[i] > punt->datamax) {
punt->datamax = punt->pixels[i];
}
}
do_write_fits (fL->cmb_image, "!cmb.fits");
do_write_fits (punt, argv[3]);
mockcbi_sub (fL->cmb_image, fL->fg_image,
fL->header_obs[0], fL->samples_obs[0], fL->samples_mod[0],
fL->beam);
seed = -labs((long)time(0));
add_noise (fL->header_obs[0],
fL->samples_mod[0], &seed);
do_write ("write uvf", argv[4], fL->header_obs[0],
fL->samples_mod[0], argv[2]);
*/
return 0;
}
void rm_test_0(ull r, ull m, int k, int max_iter, FILE *fout) {
ull wordsize = 0, dim = 0, gen_size = 0;
unsigned char *G = NULL, *D = NULL, *DD = NULL, *C = NULL, *NC = NULL;
unsigned short *masks = NULL, width = 0;
ull accumulator = 0, n = 0;
ull data_block_size_bytes = 0;
ull code_block_size_bytes = 0;
dim = rm_dim(r, m);
wordsize = rm_wordsize(r, m);
width = rm_codeword_size_in_bytes(r, m);
gen_size = gen_matrix_mem_size(m, m);
G = (unsigned char *) malloc(gen_size);
generate_matrix(m, m, G);
//print(dim, 8 * width, G);printf("\n");
masks = (unsigned short *) malloc(wordsize * sizeof(unsigned short));
generate_masks(m, masks);
data_block_size_bytes = rm_dataword_size_in_bytes(r, m);
code_block_size_bytes = rm_codeword_size_in_bytes(r, m);
D = (unsigned char *) malloc(data_block_size_bytes);
DD = (unsigned char *) malloc(data_block_size_bytes);
for (int i = 0; i < data_block_size_bytes; ++i)
D[i] = (unsigned char) rand();
C = (unsigned char *) malloc(code_block_size_bytes);
NC = (unsigned char *) malloc(code_block_size_bytes);
encode_block(r, m, dim, wordsize, C, D, masks, G);
//printf("Data:\n");print(1, dim, D);printf("\n");
memcpy(NC, C, width);
// print(1, wordsize, C);printf("\n");
// decode_block(r, m, dim, wordsize, NC, DD, masks, G, width);
// print(1, dim, DD);
srand (time(NULL));
n = 2 * k / 3 + 1;
fprintf(fout, "%lld\n", n);
for (ull i = 0; i < n; i++) {
accumulator = 0;
fprintf(fout, "%1.9lf ", (double) i / (double) k);
for (ull j = 0; j < max_iter; ++j) {
memcpy(NC, C, code_block_size_bytes);
add_noise(NC, code_block_size_bytes, (double) i / (double) k);
decode_block(r, m, dim, wordsize, NC, DD, masks, G, width);
accumulator += match(data_block_size_bytes, 0, dim, D, DD);
}
fprintf(fout, "%1.9lf\n", (double) accumulator / (double) max_iter);
}
FREE_IF_ALLOCATED(C);
FREE_IF_ALLOCATED(NC);
FREE_IF_ALLOCATED(D);
FREE_IF_ALLOCATED(DD);
FREE_IF_ALLOCATED(masks);
FREE_IF_ALLOCATED(G);
}
