void pos_update(pos_t *out, pos_in_t *in) { ASSERT_NOT_NULL(out); ASSERT_NOT_NULL(in); /* run kalman filters: */ kalman_run(&n_kalman, &out->ne_pos.n, &out->ne_speed.n, in->pos_n, in->speed_n, in->acc.n, in->dt); kalman_run(&e_kalman, &out->ne_pos.e, &out->ne_speed.e, in->pos_e, in->speed_e, in->acc.e, in->dt); kalman_run(&baro_u_kalman, &out->baro_u.pos, &out->baro_u.speed, in->baro_u, 0.0f, in->acc.u, in->dt); if (fabs(in->ultra_u - ultra_prev) > 10.0 * fabs(in->baro_u - baro_prev)) kalman_run(&ultra_u_kalman, &out->ultra_u.pos, &out->ultra_u.speed, ultra_prev + in->baro_u - baro_prev, 0.0f, in->acc.u, in->dt); else kalman_run(&ultra_u_kalman, &out->ultra_u.pos, &out->ultra_u.speed, in->ultra_u, 0.0f, in->acc.u, in->dt); baro_prev = out->baro_u.pos; ultra_prev = out->ultra_u.pos; }
int main(void) { srand(1); kalman_out_t kalman_state = {100.0, -10.0}; kalman_config_t lateral_kalman_config = { 0.03, 0.001, 1.01 }; /* create kalman filters: */ kalman_t *kalman = kalman_create(&lateral_kalman_config, &kalman_state); int i; for (i = 0; i < 10000; i++) { float z = 5.0 + 2 * (float)rand() / (float)RAND_MAX + i / 100.0; float a = 1.0 - 0.5 * (float)rand() / (float)RAND_MAX; kalman_in_t in = {z, a}; kalman_out_t out; kalman_run(&out, kalman, &in); printf("%f, %f, %f\n", z, out.pos, out.speed); } return 0; }
int main(void) { i2c_bus_t bus; int ret = i2c_bus_open(&bus, "/dev/i2c-3"); if (ret < 0) { fatal("could not open i2c bus", ret); return EXIT_FAILURE; } /* ITG: */ itg3200_dev_t itg; itg_again: ret = itg3200_init(&itg, &bus, ITG3200_DLPF_42HZ); if (ret < 0) { fatal("could not inizialize ITG3200", ret); if (ret == -EAGAIN) { goto itg_again; } return EXIT_FAILURE; } /* BMA: */ bma180_dev_t bma; bma180_init(&bma, &bus, BMA180_RANGE_4G, BMA180_BW_10HZ); /* HMC: */ hmc5883_dev_t hmc; hmc5883_init(&hmc, &bus); /* MS: */ ms5611_dev_t ms; ret = ms5611_init(&ms, &bus, MS5611_OSR4096, MS5611_OSR4096); if (ret < 0) { fatal("could not inizialize MS5611", ret); return EXIT_FAILURE; } pthread_t thread; pthread_create(&thread, NULL, ms5611_reader, &ms); /* initialize AHRS filter: */ madgwick_ahrs_t madgwick_ahrs; madgwick_ahrs_init(&madgwick_ahrs, STANDARD_BETA); interval_t interval; interval_init(&interval); float init = START_BETA; udp_socket_t *socket = udp_socket_create("10.0.0.100", 5005, 0, 0); /* kalman filter: */ kalman_t kalman1, kalman2, kalman3; kalman_init(&kalman1, 1.0e-6, 1.0e-2, 0, 0); kalman_init(&kalman2, 1.0e-6, 1.0e-2, 0, 0); kalman_init(&kalman3, 1.0e-6, 1.0e-2, 0, 0); vec3_t global_acc; /* x = N, y = E, z = D */ int init_done = 0; int converged = 0; sliding_avg_t *avg[3]; avg[0] = sliding_avg_create(1000, 0.0); avg[1] = sliding_avg_create(1000, 0.0); avg[2] = sliding_avg_create(1000, -9.81); float alt_rel_last = 0.0; int udp_cnt = 0; while (1) { int i; float dt = interval_measure(&interval); init -= BETA_STEP; if (init < FINAL_BETA) { init = FINAL_BETA; init_done = 1; } madgwick_ahrs.beta = init; /* sensor data acquisition: */ itg3200_read_gyro(&itg); bma180_read_acc(&bma); hmc5883_read(&hmc); /* state estimates and output: */ euler_t euler; madgwick_ahrs_update(&madgwick_ahrs, itg.gyro.x, itg.gyro.y, itg.gyro.z, bma.raw.x, bma.raw.y, bma.raw.z, hmc.raw.x, hmc.raw.y, hmc.raw.z, 11.0, dt); quat_t q_body_to_world; quat_copy(&q_body_to_world, &madgwick_ahrs.quat); quat_rot_vec(&global_acc, &bma.raw, &q_body_to_world); for (i = 0; i < 3; i++) { global_acc.vec[i] -= sliding_avg_calc(avg[i], global_acc.vec[i]); } if (init_done) { kalman_in_t kalman_in; kalman_in.dt = dt; kalman_in.pos = 0; kalman_out_t kalman_out; kalman_in.acc = global_acc.x; kalman_run(&kalman_out, &kalman1, &kalman_in); kalman_in.acc = global_acc.y; kalman_run(&kalman_out, &kalman2, &kalman_in); kalman_in.acc = -global_acc.z; pthread_mutex_lock(&mutex); kalman_in.pos = alt_rel; pthread_mutex_unlock(&mutex); kalman_run(&kalman_out, &kalman3, &kalman_in); if (!converged) { if (fabs(kalman_out.pos - alt_rel) < 0.1) { converged = 1; fprintf(stderr, "init done\n"); } } if (converged) // && udp_cnt++ > 10) { if (udp_cnt++ == 10) { char buffer[1024]; udp_cnt = 0; int len = sprintf(buffer, "%f %f %f %f %f %f %f", madgwick_ahrs.quat.q0, madgwick_ahrs.quat.q1, madgwick_ahrs.quat.q2, madgwick_ahrs.quat.q3, global_acc.x, global_acc.y, global_acc.z); udp_socket_send(socket, buffer, len); } printf("%f %f %f\n", -global_acc.z, alt_rel, kalman_out.pos); fflush(stdout); } } } return 0; }