From fef4362e79fb05ddf8caba4bb4365bab13b39ce6 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 16 Oct 2012 11:10:09 +0200 Subject: Merged new EKF version --- apps/attitude_estimator_ekf/.context | 0 apps/attitude_estimator_ekf/Makefile | 5 +- .../attitude_estimator_ekf_main.c | 388 +++++++++-------- .../attitude_estimator_ekf_params.c | 12 +- .../attitude_estimator_ekf_params.h | 0 .../codegen/attitudeKalmanfilter.c | 463 +++++++++++---------- .../codegen/attitudeKalmanfilter.h | 4 +- .../codegen/attitudeKalmanfilter_initialize.c | 2 +- .../codegen/attitudeKalmanfilter_initialize.h | 4 +- .../codegen/attitudeKalmanfilter_terminate.c | 2 +- .../codegen/attitudeKalmanfilter_terminate.h | 4 +- .../codegen/attitudeKalmanfilter_types.h | 2 +- apps/attitude_estimator_ekf/codegen/cross.c | 2 +- apps/attitude_estimator_ekf/codegen/cross.h | 4 +- apps/attitude_estimator_ekf/codegen/diag.c | 28 +- apps/attitude_estimator_ekf/codegen/diag.h | 11 +- apps/attitude_estimator_ekf/codegen/eye.c | 6 +- apps/attitude_estimator_ekf/codegen/eye.h | 4 +- apps/attitude_estimator_ekf/codegen/mrdivide.c | 218 +++++----- apps/attitude_estimator_ekf/codegen/mrdivide.h | 4 +- apps/attitude_estimator_ekf/codegen/norm.c | 30 +- apps/attitude_estimator_ekf/codegen/norm.h | 4 +- apps/attitude_estimator_ekf/codegen/power.c | 84 ---- apps/attitude_estimator_ekf/codegen/power.h | 34 -- apps/attitude_estimator_ekf/codegen/rdivide.c | 38 ++ apps/attitude_estimator_ekf/codegen/rdivide.h | 34 ++ apps/attitude_estimator_ekf/codegen/rtGetInf.c | 2 +- apps/attitude_estimator_ekf/codegen/rtGetInf.h | 2 +- apps/attitude_estimator_ekf/codegen/rtGetNaN.c | 2 +- apps/attitude_estimator_ekf/codegen/rtGetNaN.h | 2 +- apps/attitude_estimator_ekf/codegen/rt_defines.h | 2 +- apps/attitude_estimator_ekf/codegen/rt_nonfinite.c | 2 +- apps/attitude_estimator_ekf/codegen/rt_nonfinite.h | 2 +- apps/attitude_estimator_ekf/codegen/rtwtypes.h | 6 +- 34 files changed, 696 insertions(+), 711 deletions(-) delete mode 100644 apps/attitude_estimator_ekf/.context mode change 100644 => 100755 apps/attitude_estimator_ekf/Makefile mode change 100644 => 100755 apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c mode change 100644 => 100755 apps/attitude_estimator_ekf/attitude_estimator_ekf_params.c mode change 100644 => 100755 apps/attitude_estimator_ekf/attitude_estimator_ekf_params.h delete mode 100755 apps/attitude_estimator_ekf/codegen/power.c delete mode 100755 apps/attitude_estimator_ekf/codegen/power.h create mode 100755 apps/attitude_estimator_ekf/codegen/rdivide.c create mode 100755 apps/attitude_estimator_ekf/codegen/rdivide.h (limited to 'apps/attitude_estimator_ekf') diff --git a/apps/attitude_estimator_ekf/.context b/apps/attitude_estimator_ekf/.context deleted file mode 100644 index e69de29bb..000000000 diff --git a/apps/attitude_estimator_ekf/Makefile b/apps/attitude_estimator_ekf/Makefile old mode 100644 new mode 100755 index e5620138a..d6ec98f0b --- a/apps/attitude_estimator_ekf/Makefile +++ b/apps/attitude_estimator_ekf/Makefile @@ -40,6 +40,7 @@ CSRCS = attitude_estimator_ekf_main.c \ codegen/eye.c \ codegen/attitudeKalmanfilter.c \ codegen/mrdivide.c \ + codegen/rdivide.c \ codegen/attitudeKalmanfilter_initialize.c \ codegen/attitudeKalmanfilter_terminate.c \ codegen/rt_nonfinite.c \ @@ -47,8 +48,8 @@ CSRCS = attitude_estimator_ekf_main.c \ codegen/rtGetNaN.c \ codegen/norm.c \ codegen/diag.c \ - codegen/cross.c \ - codegen/power.c + codegen/cross.c + # XXX this is *horribly* broken INCLUDES += $(TOPDIR)/../mavlink/include/mavlink diff --git a/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c b/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c old mode 100644 new mode 100755 index 1d4df87fe..b507b4c10 --- a/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c +++ b/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c @@ -77,40 +77,40 @@ static float z_k[9]; /**< Measurement vector */ static float x_aposteriori_k[12]; /**< */ static float x_aposteriori[12]; static float P_aposteriori_k[144] = {100.f, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 100.f, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 100.f, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 0, 100.f, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 0, 0, 100.f, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 0, 0, 0, 100.f, 0, 0, 0, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 100.f, 0, 0, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 100.f, 0, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 0, 100.f, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 0, 0.0f, 100.0f, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 0, 0.0f, 0, 100.0f, 0, - 0, 0, 0, 0, 0, 0, 0, 0, 0.0f, 0, 0, 100.0f, - }; + 0, 100.f, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 100.f, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 100.f, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 100.f, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 100.f, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 100.f, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 100.f, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 100.f, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0.0f, 100.0f, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0.0f, 0, 100.0f, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0.0f, 0, 0, 100.0f, +}; static float P_aposteriori[144] = {100.f, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 100.f, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 100.f, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 0, 100.f, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 0, 0, 100.f, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 0, 0, 0, 100.f, 0, 0, 0, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 100.f, 0, 0, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 100.f, 0, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 0, 100.f, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 0, 0.0f, 100.0f, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 0, 0.0f, 0, 100.0f, 0, - 0, 0, 0, 0, 0, 0, 0, 0, 0.0f, 0, 0, 100.0f, - }; /**< init: diagonal matrix with big values */ + 0, 100.f, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 100.f, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 100.f, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 100.f, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 100.f, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 100.f, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 100.f, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 100.f, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0.0f, 100.0f, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0.0f, 0, 100.0f, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0.0f, 0, 0, 100.0f, +}; /**< init: diagonal matrix with big values */ /* output euler angles */ static float euler[3] = {0.0f, 0.0f, 0.0f}; static float Rot_matrix[9] = {1.f, 0, 0, - 0, 1.f, 0, - 0, 0, 1.f - }; /**< init: identity matrix */ + 0, 1.f, 0, + 0, 0, 1.f +}; /**< init: identity matrix */ static bool thread_should_exit = false; /**< Deamon exit flag */ @@ -159,11 +159,11 @@ int attitude_estimator_ekf_main(int argc, char *argv[]) thread_should_exit = false; attitude_estimator_ekf_task = task_spawn("attitude_estimator_ekf", - SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 5, - 20000, - attitude_estimator_ekf_thread_main, - (argv) ? (const char **)&argv[2] : (const char **)NULL); + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 5, + 20000, + attitude_estimator_ekf_thread_main, + (argv) ? (const char **)&argv[2] : (const char **)NULL); exit(0); } @@ -250,12 +250,18 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[]) /* initialize parameter handles */ parameters_init(&ekf_param_handles); + uint64_t start_time = hrt_absolute_time(); + bool initialized = false; + + float gyro_offsets[3] = { 0.0f, 0.0f, 0.0f }; + unsigned offset_count = 0; + /* Main loop*/ while (!thread_should_exit) { struct pollfd fds[2] = { - { .fd = sub_raw, .events = POLLIN }, - { .fd = sub_params, .events = POLLIN } + { .fd = sub_raw, .events = POLLIN }, + { .fd = sub_params, .events = POLLIN } }; int ret = poll(fds, 2, 1000); @@ -265,7 +271,7 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[]) /* XXX this means no sensor data - should be critical or emergency */ printf("[attitude estimator ekf] WARNING: Not getting sensor data - sensor app running?\n"); } else { - + /* only update parameters if they changed */ if (fds[1].revents & POLLIN) { /* read from param to clear updated flag */ @@ -282,156 +288,170 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[]) /* get latest measurements */ orb_copy(ORB_ID(sensor_combined), sub_raw, &raw); - /* Calculate data time difference in seconds */ - dt = (raw.timestamp - last_measurement) / 1000000.0f; - last_measurement = raw.timestamp; - uint8_t update_vect[3] = {0, 0, 0}; - - /* Fill in gyro measurements */ - if (sensor_last_count[0] != raw.gyro_counter) { - update_vect[0] = 1; - sensor_last_count[0] = raw.gyro_counter; - sensor_update_hz[0] = 1e6f / (raw.timestamp - sensor_last_timestamp[0]); - sensor_last_timestamp[0] = raw.timestamp; - } - - z_k[0] = raw.gyro_rad_s[0]; - z_k[1] = raw.gyro_rad_s[1]; - z_k[2] = raw.gyro_rad_s[2]; - - /* update accelerometer measurements */ - if (sensor_last_count[1] != raw.accelerometer_counter) { - update_vect[1] = 1; - sensor_last_count[1] = raw.accelerometer_counter; - sensor_update_hz[1] = 1e6f / (raw.timestamp - sensor_last_timestamp[1]); - sensor_last_timestamp[1] = raw.timestamp; - } - z_k[3] = raw.accelerometer_m_s2[0]; - z_k[4] = raw.accelerometer_m_s2[1]; - z_k[5] = raw.accelerometer_m_s2[2]; - - /* update magnetometer measurements */ - if (sensor_last_count[2] != raw.magnetometer_counter) { - update_vect[2] = 1; - sensor_last_count[2] = raw.magnetometer_counter; - sensor_update_hz[2] = 1e6f / (raw.timestamp - sensor_last_timestamp[2]); - sensor_last_timestamp[2] = raw.timestamp; - } - z_k[6] = raw.magnetometer_ga[0]; - z_k[7] = raw.magnetometer_ga[1]; - z_k[8] = raw.magnetometer_ga[2]; - - uint64_t now = hrt_absolute_time(); - unsigned int time_elapsed = now - last_run; - last_run = now; - - if (time_elapsed > loop_interval_alarm) { - //TODO: add warning, cpu overload here - // if (overloadcounter == 20) { - // printf("CPU OVERLOAD DETECTED IN ATTITUDE ESTIMATOR EKF (%lu > %lu)\n", time_elapsed, loop_interval_alarm); - // overloadcounter = 0; - // } - - overloadcounter++; - } - - int32_t z_k_sizes = 9; - // float u[4] = {0.0f, 0.0f, 0.0f, 0.0f}; - - static bool const_initialized = false; - - /* initialize with good values once we have a reasonable dt estimate */ - if (!const_initialized /*&& dt < 0.05 && dt > 0.005*/) - { - dt = 0.005f; - parameters_update(&ekf_param_handles, &ekf_params); - - x_aposteriori_k[0] = z_k[0]; - x_aposteriori_k[1] = z_k[1]; - x_aposteriori_k[2] = z_k[2]; - x_aposteriori_k[3] = 0.0f; - x_aposteriori_k[4] = 0.0f; - x_aposteriori_k[5] = 0.0f; - x_aposteriori_k[6] = z_k[3]; - x_aposteriori_k[7] = z_k[4]; - x_aposteriori_k[8] = z_k[5]; - x_aposteriori_k[9] = z_k[6]; - x_aposteriori_k[10] = z_k[7]; - x_aposteriori_k[11] = z_k[8]; - - const_initialized = true; - } - - /* do not execute the filter if not initialized */ - if (!const_initialized) { - continue; + if (!initialized) { + + gyro_offsets[0] += raw.gyro_rad_s[0]; + gyro_offsets[1] += raw.gyro_rad_s[1]; + gyro_offsets[2] += raw.gyro_rad_s[2]; + + if (hrt_absolute_time() - start_time > 3000000LL) { + initialized = true; + gyro_offsets[0] /= offset_count; + gyro_offsets[1] /= offset_count; + gyro_offsets[2] /= offset_count; + } + } else { + + /* Calculate data time difference in seconds */ + dt = (raw.timestamp - last_measurement) / 1000000.0f; + last_measurement = raw.timestamp; + uint8_t update_vect[3] = {0, 0, 0}; + + /* Fill in gyro measurements */ + if (sensor_last_count[0] != raw.gyro_counter) { + update_vect[0] = 1; + sensor_last_count[0] = raw.gyro_counter; + sensor_update_hz[0] = 1e6f / (raw.timestamp - sensor_last_timestamp[0]); + sensor_last_timestamp[0] = raw.timestamp; + } + + z_k[0] = raw.gyro_rad_s[0]; + z_k[1] = raw.gyro_rad_s[1]; + z_k[2] = raw.gyro_rad_s[2]; + + /* update accelerometer measurements */ + if (sensor_last_count[1] != raw.accelerometer_counter) { + update_vect[1] = 1; + sensor_last_count[1] = raw.accelerometer_counter; + sensor_update_hz[1] = 1e6f / (raw.timestamp - sensor_last_timestamp[1]); + sensor_last_timestamp[1] = raw.timestamp; + } + z_k[3] = raw.accelerometer_m_s2[0]; + z_k[4] = raw.accelerometer_m_s2[1]; + z_k[5] = raw.accelerometer_m_s2[2]; + + /* update magnetometer measurements */ + if (sensor_last_count[2] != raw.magnetometer_counter) { + update_vect[2] = 1; + sensor_last_count[2] = raw.magnetometer_counter; + sensor_update_hz[2] = 1e6f / (raw.timestamp - sensor_last_timestamp[2]); + sensor_last_timestamp[2] = raw.timestamp; + } + z_k[6] = raw.magnetometer_ga[0]; + z_k[7] = raw.magnetometer_ga[1]; + z_k[8] = raw.magnetometer_ga[2]; + + uint64_t now = hrt_absolute_time(); + unsigned int time_elapsed = now - last_run; + last_run = now; + + if (time_elapsed > loop_interval_alarm) { + //TODO: add warning, cpu overload here + // if (overloadcounter == 20) { + // printf("CPU OVERLOAD DETECTED IN ATTITUDE ESTIMATOR EKF (%lu > %lu)\n", time_elapsed, loop_interval_alarm); + // overloadcounter = 0; + // } + + overloadcounter++; + } + + int32_t z_k_sizes = 9; + // float u[4] = {0.0f, 0.0f, 0.0f, 0.0f}; + + static bool const_initialized = false; + + /* initialize with good values once we have a reasonable dt estimate */ + if (!const_initialized /*&& dt < 0.05 && dt > 0.005*/) + { + dt = 0.005f; + parameters_update(&ekf_param_handles, &ekf_params); + + x_aposteriori_k[0] = z_k[0]; + x_aposteriori_k[1] = z_k[1]; + x_aposteriori_k[2] = z_k[2]; + x_aposteriori_k[3] = 0.0f; + x_aposteriori_k[4] = 0.0f; + x_aposteriori_k[5] = 0.0f; + x_aposteriori_k[6] = z_k[3]; + x_aposteriori_k[7] = z_k[4]; + x_aposteriori_k[8] = z_k[5]; + x_aposteriori_k[9] = z_k[6]; + x_aposteriori_k[10] = z_k[7]; + x_aposteriori_k[11] = z_k[8]; + + const_initialized = true; + } + + /* do not execute the filter if not initialized */ + if (!const_initialized) { + continue; + } + + dt = 0.004f; + + uint64_t timing_start = hrt_absolute_time(); + // attitudeKalmanfilter(dt, update_vect, z_k, &z_k_sizes, u, x_aposteriori_k, P_aposteriori_k, knownConst, euler, + // Rot_matrix, x_aposteriori, P_aposteriori); + attitudeKalmanfilter(update_vect, dt, z_k, x_aposteriori_k, P_aposteriori_k, ekf_params.q, ekf_params.r, + euler, Rot_matrix, x_aposteriori, P_aposteriori); + /* swap values for next iteration */ + memcpy(P_aposteriori_k, P_aposteriori, sizeof(P_aposteriori_k)); + memcpy(x_aposteriori_k, x_aposteriori, sizeof(x_aposteriori_k)); + uint64_t timing_diff = hrt_absolute_time() - timing_start; + + // /* print rotation matrix every 200th time */ + if (printcounter % 200 == 0) { + // printf("x apo:\n%8.4f\t%8.4f\t%8.4f\n%8.4f\t%8.4f\t%8.4f\n%8.4f\t%8.4f\t%8.4f\n", + // x_aposteriori[0], x_aposteriori[1], x_aposteriori[2], + // x_aposteriori[3], x_aposteriori[4], x_aposteriori[5], + // x_aposteriori[6], x_aposteriori[7], x_aposteriori[8]); + + + // } + + //printf("EKF attitude iteration: %d, runtime: %d us, dt: %d us (%d Hz)\n", loopcounter, (int)timing_diff, (int)(dt * 1000000.0f), (int)(1.0f / dt)); + //printf("roll: %8.4f\tpitch: %8.4f\tyaw:%8.4f\n", (double)euler[0], (double)euler[1], (double)euler[2]); + //printf("update rates gyro: %8.4f\taccel: %8.4f\tmag:%8.4f\n", (double)sensor_update_hz[0], (double)sensor_update_hz[1], (double)sensor_update_hz[2]); + // printf("\n%d\t%d\t%d\n%d\t%d\t%d\n%d\t%d\t%d\n", (int)(Rot_matrix[0] * 100), (int)(Rot_matrix[1] * 100), (int)(Rot_matrix[2] * 100), + // (int)(Rot_matrix[3] * 100), (int)(Rot_matrix[4] * 100), (int)(Rot_matrix[5] * 100), + // (int)(Rot_matrix[6] * 100), (int)(Rot_matrix[7] * 100), (int)(Rot_matrix[8] * 100)); + } + + // int i = printcounter % 9; + + // // for (int i = 0; i < 9; i++) { + // char name[10]; + // sprintf(name, "xapo #%d", i); + // memcpy(dbg.key, name, sizeof(dbg.key)); + // dbg.value = x_aposteriori[i]; + // orb_publish(ORB_ID(debug_key_value), pub_dbg, &dbg); + + printcounter++; + + if (last_data > 0 && raw.timestamp - last_data > 12000) printf("[attitude estimator ekf] sensor data missed! (%llu)\n", raw.timestamp - last_data); + + last_data = raw.timestamp; + + /* send out */ + att.timestamp = raw.timestamp; + att.roll = euler[0]; + att.pitch = euler[1]; + att.yaw = euler[2]; + + att.rollspeed = x_aposteriori[0]; + att.pitchspeed = x_aposteriori[1]; + att.yawspeed = x_aposteriori[2]; + //att.yawspeed =z_k[2] ; + /* copy offsets */ + memcpy(&att.rate_offsets, &(x_aposteriori[3]), sizeof(att.rate_offsets)); + + /* copy rotation matrix */ + memcpy(&att.R, Rot_matrix, sizeof(Rot_matrix)); + att.R_valid = true; + + // Broadcast + orb_publish(ORB_ID(vehicle_attitude), pub_att, &att); } - - dt = 0.004f; - - uint64_t timing_start = hrt_absolute_time(); - // attitudeKalmanfilter(dt, update_vect, z_k, &z_k_sizes, u, x_aposteriori_k, P_aposteriori_k, knownConst, euler, - // Rot_matrix, x_aposteriori, P_aposteriori); - attitudeKalmanfilter(update_vect, dt, z_k, x_aposteriori_k, P_aposteriori_k, ekf_params.q, ekf_params.r, - euler, Rot_matrix, x_aposteriori, P_aposteriori); - /* swap values for next iteration */ - memcpy(P_aposteriori_k, P_aposteriori, sizeof(P_aposteriori_k)); - memcpy(x_aposteriori_k, x_aposteriori, sizeof(x_aposteriori_k)); - uint64_t timing_diff = hrt_absolute_time() - timing_start; - - // /* print rotation matrix every 200th time */ - if (printcounter % 200 == 0) { - // printf("x apo:\n%8.4f\t%8.4f\t%8.4f\n%8.4f\t%8.4f\t%8.4f\n%8.4f\t%8.4f\t%8.4f\n", - // x_aposteriori[0], x_aposteriori[1], x_aposteriori[2], - // x_aposteriori[3], x_aposteriori[4], x_aposteriori[5], - // x_aposteriori[6], x_aposteriori[7], x_aposteriori[8]); - - - // } - - //printf("EKF attitude iteration: %d, runtime: %d us, dt: %d us (%d Hz)\n", loopcounter, (int)timing_diff, (int)(dt * 1000000.0f), (int)(1.0f / dt)); - //printf("roll: %8.4f\tpitch: %8.4f\tyaw:%8.4f\n", (double)euler[0], (double)euler[1], (double)euler[2]); - //printf("update rates gyro: %8.4f\taccel: %8.4f\tmag:%8.4f\n", (double)sensor_update_hz[0], (double)sensor_update_hz[1], (double)sensor_update_hz[2]); - // printf("\n%d\t%d\t%d\n%d\t%d\t%d\n%d\t%d\t%d\n", (int)(Rot_matrix[0] * 100), (int)(Rot_matrix[1] * 100), (int)(Rot_matrix[2] * 100), - // (int)(Rot_matrix[3] * 100), (int)(Rot_matrix[4] * 100), (int)(Rot_matrix[5] * 100), - // (int)(Rot_matrix[6] * 100), (int)(Rot_matrix[7] * 100), (int)(Rot_matrix[8] * 100)); - } - - // int i = printcounter % 9; - - // // for (int i = 0; i < 9; i++) { - // char name[10]; - // sprintf(name, "xapo #%d", i); - // memcpy(dbg.key, name, sizeof(dbg.key)); - // dbg.value = x_aposteriori[i]; - // orb_publish(ORB_ID(debug_key_value), pub_dbg, &dbg); - - printcounter++; - - if (last_data > 0 && raw.timestamp - last_data > 12000) printf("[attitude estimator ekf] sensor data missed! (%llu)\n", raw.timestamp - last_data); - - last_data = raw.timestamp; - - /* send out */ - att.timestamp = raw.timestamp; - att.roll = euler[0]; - att.pitch = euler[1]; - att.yaw = euler[2]; - - // XXX replace with x_apo after fix to filter - att.rollspeed = raw.gyro_rad_s[0]; //x_aposteriori[0]; - att.pitchspeed = raw.gyro_rad_s[1]; //x_aposteriori[1]; - att.yawspeed = raw.gyro_rad_s[2]; //x_aposteriori[2]; - - /* copy offsets */ - memcpy(&att.rate_offsets, &(x_aposteriori[3]), sizeof(att.rate_offsets)); - - /* copy rotation matrix */ - memcpy(&att.R, Rot_matrix, sizeof(Rot_matrix)); - att.R_valid = true; - - // Broadcast - orb_publish(ORB_ID(vehicle_attitude), pub_att, &att); } } diff --git a/apps/attitude_estimator_ekf/attitude_estimator_ekf_params.c b/apps/attitude_estimator_ekf/attitude_estimator_ekf_params.c old mode 100644 new mode 100755 index d95c56368..f20d1b204 --- a/apps/attitude_estimator_ekf/attitude_estimator_ekf_params.c +++ b/apps/attitude_estimator_ekf/attitude_estimator_ekf_params.c @@ -61,13 +61,13 @@ PARAM_DEFINE_FLOAT(EKF_ATT_Q10, 1e-1f); PARAM_DEFINE_FLOAT(EKF_ATT_Q11, 1e-1f); /* gyro measurement noise */ -PARAM_DEFINE_FLOAT(EKF_ATT_R0, 1e-2f); -PARAM_DEFINE_FLOAT(EKF_ATT_R1, 1e-2f); -PARAM_DEFINE_FLOAT(EKF_ATT_R2, 1e-2f); +PARAM_DEFINE_FLOAT(EKF_ATT_R0, 1e-1f); +PARAM_DEFINE_FLOAT(EKF_ATT_R1, 1e-1f); +PARAM_DEFINE_FLOAT(EKF_ATT_R2, 1e-1f); /* accelerometer measurement noise */ -PARAM_DEFINE_FLOAT(EKF_ATT_R3, 1e-1f); -PARAM_DEFINE_FLOAT(EKF_ATT_R4, 1e-1f); -PARAM_DEFINE_FLOAT(EKF_ATT_R5, 1e-1f); +PARAM_DEFINE_FLOAT(EKF_ATT_R3, 1e1f); +PARAM_DEFINE_FLOAT(EKF_ATT_R4, 1e1f); +PARAM_DEFINE_FLOAT(EKF_ATT_R5, 1e1f); /* magnetometer measurement noise */ PARAM_DEFINE_FLOAT(EKF_ATT_R6, 1e-1f); PARAM_DEFINE_FLOAT(EKF_ATT_R7, 1e-1f); diff --git a/apps/attitude_estimator_ekf/attitude_estimator_ekf_params.h b/apps/attitude_estimator_ekf/attitude_estimator_ekf_params.h old mode 100644 new mode 100755 diff --git a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter.c b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter.c index 86d872fd2..26a696af2 100755 --- a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter.c +++ b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter.c @@ -3,19 +3,19 @@ * * Code generation for function 'attitudeKalmanfilter' * - * C source code generated on: Mon Oct 01 19:38:49 2012 + * C source code generated on: Sat Oct 13 17:51:09 2012 * */ /* Include files */ #include "rt_nonfinite.h" #include "attitudeKalmanfilter.h" +#include "rdivide.h" #include "norm.h" #include "cross.h" #include "eye.h" #include "mrdivide.h" #include "diag.h" -#include "power.h" /* Type Definitions */ @@ -32,10 +32,24 @@ static real32_T rt_atan2f_snf(real32_T u0, real32_T u1); static real32_T rt_atan2f_snf(real32_T u0, real32_T u1) { real32_T y; + int32_T b_u0; + int32_T b_u1; if (rtIsNaNF(u0) || rtIsNaNF(u1)) { y = ((real32_T)rtNaN); } else if (rtIsInfF(u0) && rtIsInfF(u1)) { - y = (real32_T)atan2f(u0 > 0.0F ? 1.0F : -1.0F, u1 > 0.0F ? 1.0F : -1.0F); + if (u0 > 0.0F) { + b_u0 = 1; + } else { + b_u0 = -1; + } + + if (u1 > 0.0F) { + b_u1 = 1; + } else { + b_u1 = -1; + } + + y = (real32_T)atan2((real32_T)b_u0, (real32_T)b_u1); } else if (u1 == 0.0F) { if (u0 > 0.0F) { y = RT_PIF / 2.0F; @@ -45,7 +59,7 @@ static real32_T rt_atan2f_snf(real32_T u0, real32_T u1) y = 0.0F; } } else { - y = (real32_T)atan2f(u0, u1); + y = (real32_T)atan2(u0, u1); } return y; @@ -60,30 +74,41 @@ void attitudeKalmanfilter(const uint8_T updateVect[3], real32_T dt, const eulerAngles[3], real32_T Rot_matrix[9], real32_T x_aposteriori[12], real32_T P_aposteriori[144]) { - real32_T a[12]; - int32_T i; - real32_T b_a[12]; - real32_T Q[144]; real32_T O[9]; real_T dv0[9]; - real32_T c_a[9]; - real32_T d_a[9]; + real32_T a[9]; + int32_T i; + real32_T b_a[9]; real32_T x_n_b[3]; + real32_T b_x_aposteriori_k[3]; + real32_T m_n_b[3]; real32_T z_n_b[3]; real32_T x_apriori[12]; - real32_T y_n_b[3]; int32_T i0; - real32_T e_a[3]; real_T dv1[144]; real32_T A_lin[144]; real32_T b_A_lin[144]; int32_T i1; - real32_T y; real32_T P_apriori[144]; + real32_T f0; + static const real32_T fv0[144] = { 0.1F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, + 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.1F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, + 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.1F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, + 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 1.0E-10F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, + 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 1.0E-10F, 0.0F, 0.0F, 0.0F, 0.0F, + 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 1.0E-10F, 0.0F, 0.0F, 0.0F, + 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0001F, 0.0001F, 0.0F, 0.1F, 0.0F, 0.0F, + 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0001F, 0.0001F, 0.0F, 0.0F, 0.1F, 0.0F, + 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.1F, 0.0F, + 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0001F, 0.0001F, 0.0F, 0.0F, 0.0F, 0.0F, 0.1F, + 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0001F, 0.0001F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, + 0.1F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, + 0.1F }; + real32_T R[81]; real32_T b_P_apriori[108]; static const int8_T iv0[108] = { 1, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, - 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1 }; @@ -91,46 +116,45 @@ void attitudeKalmanfilter(const uint8_T updateVect[3], real32_T dt, const real32_T K_k[108]; static const int8_T iv1[108] = { 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1 }; - real32_T fv0[81]; + real32_T fv1[81]; real32_T c_P_apriori[36]; static const int8_T iv2[36] = { 1, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, - 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 1, 0, 0, 0, 0, 0, 0 }; + 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0 }; - real32_T fv1[36]; + real32_T fv2[36]; static const int8_T iv3[36] = { 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, - 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 }; + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 }; real32_T S_k[36]; real32_T d_P_apriori[72]; static const int8_T iv4[72] = { 1, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, - 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0 }; real32_T b_K_k[72]; static const int8_T iv5[72] = { 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, - 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, + 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 }; real32_T b_r[6]; static const int8_T iv6[72] = { 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, - 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1 }; static const int8_T iv7[72] = { 1, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, - 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1 }; - real32_T fv2[6]; + real32_T fv3[6]; real32_T b_z[6]; - real32_T b_y; /* Extended Attitude Kalmanfilter */ /* */ @@ -146,32 +170,37 @@ void attitudeKalmanfilter(const uint8_T updateVect[3], real32_T dt, const /* coder.varsize('udpIndVect', [9,1], [1,0]) */ /* udpIndVect=find(updVect); */ /* process and measurement noise covariance matrix */ - /* 'attitudeKalmanfilter:27' Q = diag(q.^2*dt); */ - power(q, 2.0, a); - for (i = 0; i < 12; i++) { - b_a[i] = a[i] * dt; - } - - diag(b_a, Q); - + /* Q = diag(q.^2*dt); */ + /* 'attitudeKalmanfilter:29' Q=[1e-1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0; */ + /* 'attitudeKalmanfilter:30' 0, 1e-1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0; */ + /* 'attitudeKalmanfilter:31' 0, 0, 1e-1, 0, 0, 0, 0, 0, 0, 0, 0, 0; */ + /* 'attitudeKalmanfilter:32' 0, 0, 0, 1e-10, 0, 0, 1e-4, 1e-4, 0, 1e-4, 1e-4, 0; */ + /* 'attitudeKalmanfilter:33' 0, 0, 0, 0, 1e-10, 0, 1e-4, 1e-4, 0, 1e-4, 1e-4, 0; */ + /* 'attitudeKalmanfilter:34' 0, 0, 0, 0, 0, 1e-10, 0, 0, 0, 0, 0, 0; */ + /* 'attitudeKalmanfilter:35' 0, 0, 0, 0, 0, 0, 1e-1, 0, 0, 0, 0, 0; */ + /* 'attitudeKalmanfilter:36' 0, 0, 0, 0, 0, 0, 0, 1e-1, 0, 0, 0, 0; */ + /* 'attitudeKalmanfilter:37' 0, 0, 0, 0, 0, 0, 0, 0, 1e-1, 0, 0, 0; */ + /* 'attitudeKalmanfilter:38' 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-1, 0, 0; */ + /* 'attitudeKalmanfilter:39' 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-1, 0; */ + /* 'attitudeKalmanfilter:40' 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-1]; */ /* observation matrix */ - /* 'attitudeKalmanfilter:37' wx= x_aposteriori_k(1); */ - /* 'attitudeKalmanfilter:38' wy= x_aposteriori_k(2); */ - /* 'attitudeKalmanfilter:39' wz= x_aposteriori_k(3); */ - /* 'attitudeKalmanfilter:41' wox= x_aposteriori_k(4); */ - /* 'attitudeKalmanfilter:42' woy= x_aposteriori_k(5); */ - /* 'attitudeKalmanfilter:43' woz= x_aposteriori_k(6); */ - /* 'attitudeKalmanfilter:45' zex= x_aposteriori_k(7); */ - /* 'attitudeKalmanfilter:46' zey= x_aposteriori_k(8); */ - /* 'attitudeKalmanfilter:47' zez= x_aposteriori_k(9); */ - /* 'attitudeKalmanfilter:49' mux= x_aposteriori_k(10); */ - /* 'attitudeKalmanfilter:50' muy= x_aposteriori_k(11); */ - /* 'attitudeKalmanfilter:51' muz= x_aposteriori_k(12); */ - /* 'attitudeKalmanfilter:54' wk =[wx; */ - /* 'attitudeKalmanfilter:55' wy; */ - /* 'attitudeKalmanfilter:56' wz]; */ - /* 'attitudeKalmanfilter:58' wok =[wox;woy;woz]; */ - /* 'attitudeKalmanfilter:59' O=[0,-wz,wy;wz,0,-wx;-wy,wx,0]'; */ + /* 'attitudeKalmanfilter:44' wx= x_aposteriori_k(1); */ + /* 'attitudeKalmanfilter:45' wy= x_aposteriori_k(2); */ + /* 'attitudeKalmanfilter:46' wz= x_aposteriori_k(3); */ + /* 'attitudeKalmanfilter:48' wox= x_aposteriori_k(4); */ + /* 'attitudeKalmanfilter:49' woy= x_aposteriori_k(5); */ + /* 'attitudeKalmanfilter:50' woz= x_aposteriori_k(6); */ + /* 'attitudeKalmanfilter:52' zex= x_aposteriori_k(7); */ + /* 'attitudeKalmanfilter:53' zey= x_aposteriori_k(8); */ + /* 'attitudeKalmanfilter:54' zez= x_aposteriori_k(9); */ + /* 'attitudeKalmanfilter:56' mux= x_aposteriori_k(10); */ + /* 'attitudeKalmanfilter:57' muy= x_aposteriori_k(11); */ + /* 'attitudeKalmanfilter:58' muz= x_aposteriori_k(12); */ + /* 'attitudeKalmanfilter:61' wk =[wx; */ + /* 'attitudeKalmanfilter:62' wy; */ + /* 'attitudeKalmanfilter:63' wz]; */ + /* 'attitudeKalmanfilter:65' wok =[wox;woy;woz]; */ + /* 'attitudeKalmanfilter:66' O=[0,-wz,wy;wz,0,-wx;-wy,wx,0]'; */ O[0] = 0.0F; O[1] = -x_aposteriori_k[2]; O[2] = x_aposteriori_k[1]; @@ -182,33 +211,36 @@ void attitudeKalmanfilter(const uint8_T updateVect[3], real32_T dt, const O[7] = x_aposteriori_k[0]; O[8] = 0.0F; - /* 'attitudeKalmanfilter:60' zek =(eye(3)+O*dt)*[zex;zey;zez]; */ + /* 'attitudeKalmanfilter:67' zek =(eye(3)+O*dt)*[zex;zey;zez]; */ eye(dv0); for (i = 0; i < 9; i++) { - c_a[i] = (real32_T)dv0[i] + O[i] * dt; + a[i] = (real32_T)dv0[i] + O[i] * dt; } - /* 'attitudeKalmanfilter:61' muk =(eye(3)+O*dt)*[mux;muy;muz]; */ + /* 'attitudeKalmanfilter:68' muk =(eye(3)+O*dt)*[mux;muy;muz]; */ eye(dv0); for (i = 0; i < 9; i++) { - d_a[i] = (real32_T)dv0[i] + O[i] * dt; + b_a[i] = (real32_T)dv0[i] + O[i] * dt; } - /* 'attitudeKalmanfilter:63' EZ=[0,zez,-zey; */ - /* 'attitudeKalmanfilter:64' -zez,0,zex; */ - /* 'attitudeKalmanfilter:65' zey,-zex,0]'; */ - /* 'attitudeKalmanfilter:66' MA=[0,muz,-muy; */ - /* 'attitudeKalmanfilter:67' -muz,0,mux; */ - /* 'attitudeKalmanfilter:68' zey,-mux,0]'; */ - /* 'attitudeKalmanfilter:72' E=eye(3); */ - /* 'attitudeKalmanfilter:73' Z=zeros(3); */ - /* 'attitudeKalmanfilter:74' x_apriori=[wk;wok;zek;muk]; */ + /* 'attitudeKalmanfilter:70' EZ=[0,zez,-zey; */ + /* 'attitudeKalmanfilter:71' -zez,0,zex; */ + /* 'attitudeKalmanfilter:72' zey,-zex,0]'; */ + /* 'attitudeKalmanfilter:73' MA=[0,muz,-muy; */ + /* 'attitudeKalmanfilter:74' -muz,0,mux; */ + /* 'attitudeKalmanfilter:75' zey,-mux,0]'; */ + /* 'attitudeKalmanfilter:79' E=eye(3); */ + /* 'attitudeKalmanfilter:80' Es=[1,0,0; */ + /* 'attitudeKalmanfilter:81' 0,1,0; */ + /* 'attitudeKalmanfilter:82' 0,0,0]; */ + /* 'attitudeKalmanfilter:83' Z=zeros(3); */ + /* 'attitudeKalmanfilter:84' x_apriori=[wk;wok;zek;muk]; */ x_n_b[0] = x_aposteriori_k[6]; x_n_b[1] = x_aposteriori_k[7]; x_n_b[2] = x_aposteriori_k[8]; - z_n_b[0] = x_aposteriori_k[9]; - z_n_b[1] = x_aposteriori_k[10]; - z_n_b[2] = x_aposteriori_k[11]; + b_x_aposteriori_k[0] = x_aposteriori_k[9]; + b_x_aposteriori_k[1] = x_aposteriori_k[10]; + b_x_aposteriori_k[2] = x_aposteriori_k[11]; x_apriori[0] = x_aposteriori_k[0]; x_apriori[1] = x_aposteriori_k[1]; x_apriori[2] = x_aposteriori_k[2]; @@ -216,28 +248,28 @@ void attitudeKalmanfilter(const uint8_T updateVect[3], real32_T dt, const x_apriori[4] = x_aposteriori_k[4]; x_apriori[5] = x_aposteriori_k[5]; for (i = 0; i < 3; i++) { - y_n_b[i] = 0.0F; + m_n_b[i] = 0.0F; for (i0 = 0; i0 < 3; i0++) { - y_n_b[i] += c_a[i + 3 * i0] * x_n_b[i0]; + m_n_b[i] += a[i + 3 * i0] * x_n_b[i0]; } - e_a[i] = 0.0F; + z_n_b[i] = 0.0F; for (i0 = 0; i0 < 3; i0++) { - e_a[i] += d_a[i + 3 * i0] * z_n_b[i0]; + z_n_b[i] += b_a[i + 3 * i0] * b_x_aposteriori_k[i0]; } - x_apriori[i + 6] = y_n_b[i]; + x_apriori[i + 6] = m_n_b[i]; } for (i = 0; i < 3; i++) { - x_apriori[i + 9] = e_a[i]; + x_apriori[i + 9] = z_n_b[i]; } - /* 'attitudeKalmanfilter:76' A_lin=[ Z, Z, Z, Z */ - /* 'attitudeKalmanfilter:77' Z, Z, Z, Z */ - /* 'attitudeKalmanfilter:78' EZ, Z, O, Z */ - /* 'attitudeKalmanfilter:79' MA, Z, Z, O]; */ - /* 'attitudeKalmanfilter:82' A_lin=eye(12)+A_lin*dt; */ + /* 'attitudeKalmanfilter:86' A_lin=[ Z, Z, Z, Z */ + /* 'attitudeKalmanfilter:87' Z, Z, Z, Z */ + /* 'attitudeKalmanfilter:88' EZ, Z, O, Z */ + /* 'attitudeKalmanfilter:89' MA, Z, Z, O]; */ + /* 'attitudeKalmanfilter:92' A_lin=eye(12)+A_lin*dt; */ b_eye(dv1); for (i = 0; i < 12; i++) { for (i0 = 0; i0 < 3; i0++) { @@ -310,7 +342,7 @@ void attitudeKalmanfilter(const uint8_T updateVect[3], real32_T dt, const } } - /* 'attitudeKalmanfilter:88' P_apriori=A_lin*P_aposteriori_k*A_lin'+Q; */ + /* 'attitudeKalmanfilter:98' P_apriori=A_lin*P_aposteriori_k*A_lin'+Q; */ for (i = 0; i < 12; i++) { for (i0 = 0; i0 < 12; i0++) { A_lin[i + 12 * i0] = 0.0F; @@ -323,28 +355,28 @@ void attitudeKalmanfilter(const uint8_T updateVect[3], real32_T dt, const for (i = 0; i < 12; i++) { for (i0 = 0; i0 < 12; i0++) { - y = 0.0F; + f0 = 0.0F; for (i1 = 0; i1 < 12; i1++) { - y += A_lin[i + 12 * i1] * b_A_lin[i0 + 12 * i1]; + f0 += A_lin[i + 12 * i1] * b_A_lin[i0 + 12 * i1]; } - P_apriori[i + 12 * i0] = y + Q[i + 12 * i0]; + P_apriori[i + 12 * i0] = f0 + fv0[i + 12 * i0]; } } /* %update */ - /* 'attitudeKalmanfilter:92' if updateVect(1)==1&&updateVect(2)==1&&updateVect(3)==1 */ + /* 'attitudeKalmanfilter:102' if updateVect(1)==1&&updateVect(2)==1&&updateVect(3)==1 */ if ((updateVect[0] == 1) && (updateVect[1] == 1) && (updateVect[2] == 1)) { - /* 'attitudeKalmanfilter:93' R=diag(r); */ - b_diag(r, R); + /* 'attitudeKalmanfilter:103' R=diag(r); */ + diag(r, R); /* observation matrix */ - /* 'attitudeKalmanfilter:96' H_k=[ E, E, Z, Z; */ - /* 'attitudeKalmanfilter:97' Z, Z, E, Z; */ - /* 'attitudeKalmanfilter:98' Z, Z, Z, E]; */ - /* 'attitudeKalmanfilter:100' y_k=z(1:9)-H_k*x_apriori; */ - /* 'attitudeKalmanfilter:102' S_k=H_k*P_apriori*H_k'+R; */ - /* 'attitudeKalmanfilter:103' K_k=(P_apriori*H_k'/(S_k)); */ + /* 'attitudeKalmanfilter:106' H_k=[ E, Es, Z, Z; */ + /* 'attitudeKalmanfilter:107' Z, Z, E, Z; */ + /* 'attitudeKalmanfilter:108' Z, Z, Z, E]; */ + /* 'attitudeKalmanfilter:110' y_k=z(1:9)-H_k*x_apriori; */ + /* 'attitudeKalmanfilter:112' S_k=H_k*P_apriori*H_k'+R; */ + /* 'attitudeKalmanfilter:113' K_k=(P_apriori*H_k'/(S_k)); */ for (i = 0; i < 12; i++) { for (i0 = 0; i0 < 9; i0++) { b_P_apriori[i + 12 * i0] = 0.0F; @@ -366,46 +398,46 @@ void attitudeKalmanfilter(const uint8_T updateVect[3], real32_T dt, const for (i = 0; i < 9; i++) { for (i0 = 0; i0 < 9; i0++) { - y = 0.0F; + f0 = 0.0F; for (i1 = 0; i1 < 12; i1++) { - y += K_k[i + 9 * i1] * (real32_T)iv0[i1 + 12 * i0]; + f0 += K_k[i + 9 * i1] * (real32_T)iv0[i1 + 12 * i0]; } - fv0[i + 9 * i0] = y + R[i + 9 * i0]; + fv1[i + 9 * i0] = f0 + R[i + 9 * i0]; } } - mrdivide(b_P_apriori, fv0, K_k); + mrdivide(b_P_apriori, fv1, K_k); - /* 'attitudeKalmanfilter:106' x_aposteriori=x_apriori+K_k*y_k; */ + /* 'attitudeKalmanfilter:116' x_aposteriori=x_apriori+K_k*y_k; */ for (i = 0; i < 9; i++) { - y = 0.0F; + f0 = 0.0F; for (i0 = 0; i0 < 12; i0++) { - y += (real32_T)iv1[i + 9 * i0] * x_apriori[i0]; + f0 += (real32_T)iv1[i + 9 * i0] * x_apriori[i0]; } - c_a[i] = z[i] - y; + a[i] = z[i] - f0; } for (i = 0; i < 12; i++) { - y = 0.0F; + f0 = 0.0F; for (i0 = 0; i0 < 9; i0++) { - y += K_k[i + 12 * i0] * c_a[i0]; + f0 += K_k[i + 12 * i0] * a[i0]; } - x_aposteriori[i] = x_apriori[i] + y; + x_aposteriori[i] = x_apriori[i] + f0; } - /* 'attitudeKalmanfilter:107' P_aposteriori=(eye(12)-K_k*H_k)*P_apriori; */ + /* 'attitudeKalmanfilter:117' P_aposteriori=(eye(12)-K_k*H_k)*P_apriori; */ b_eye(dv1); for (i = 0; i < 12; i++) { for (i0 = 0; i0 < 12; i0++) { - y = 0.0F; + f0 = 0.0F; for (i1 = 0; i1 < 9; i1++) { - y += K_k[i + 12 * i1] * (real32_T)iv1[i1 + 9 * i0]; + f0 += K_k[i + 12 * i1] * (real32_T)iv1[i1 + 9 * i0]; } - Q[i + 12 * i0] = (real32_T)dv1[i + 12 * i0] - y; + b_A_lin[i + 12 * i0] = (real32_T)dv1[i + 12 * i0] - f0; } } @@ -413,22 +445,23 @@ void attitudeKalmanfilter(const uint8_T updateVect[3], real32_T dt, const for (i0 = 0; i0 < 12; i0++) { P_aposteriori[i + 12 * i0] = 0.0F; for (i1 = 0; i1 < 12; i1++) { - P_aposteriori[i + 12 * i0] += Q[i + 12 * i1] * P_apriori[i1 + 12 * i0]; + P_aposteriori[i + 12 * i0] += b_A_lin[i + 12 * i1] * P_apriori[i1 + 12 + * i0]; } } } } else { - /* 'attitudeKalmanfilter:108' else */ - /* 'attitudeKalmanfilter:109' if updateVect(1)==1&&updateVect(2)==0&&updateVect(3)==0 */ + /* 'attitudeKalmanfilter:118' else */ + /* 'attitudeKalmanfilter:119' if updateVect(1)==1&&updateVect(2)==0&&updateVect(3)==0 */ if ((updateVect[0] == 1) && (updateVect[1] == 0) && (updateVect[2] == 0)) { - /* 'attitudeKalmanfilter:110' R=diag(r(1:3)); */ - c_diag(*(real32_T (*)[3])&r[0], O); + /* 'attitudeKalmanfilter:120' R=diag(r(1:3)); */ + b_diag(*(real32_T (*)[3])&r[0], O); /* observation matrix */ - /* 'attitudeKalmanfilter:113' H_k=[ E, E, Z, Z]; */ - /* 'attitudeKalmanfilter:115' y_k=z(1:3)-H_k(1:3,1:12)*x_apriori; */ - /* 'attitudeKalmanfilter:117' S_k=H_k(1:3,1:12)*P_apriori*H_k(1:3,1:12)'+R(1:3,1:3); */ - /* 'attitudeKalmanfilter:118' K_k=(P_apriori*H_k(1:3,1:12)'/(S_k)); */ + /* 'attitudeKalmanfilter:123' H_k=[ E, Es, Z, Z]; */ + /* 'attitudeKalmanfilter:125' y_k=z(1:3)-H_k(1:3,1:12)*x_apriori; */ + /* 'attitudeKalmanfilter:127' S_k=H_k(1:3,1:12)*P_apriori*H_k(1:3,1:12)'+R(1:3,1:3); */ + /* 'attitudeKalmanfilter:128' K_k=(P_apriori*H_k(1:3,1:12)'/(S_k)); */ for (i = 0; i < 12; i++) { for (i0 = 0; i0 < 3; i0++) { c_P_apriori[i + 12 * i0] = 0.0F; @@ -441,9 +474,9 @@ void attitudeKalmanfilter(const uint8_T updateVect[3], real32_T dt, const for (i = 0; i < 3; i++) { for (i0 = 0; i0 < 12; i0++) { - fv1[i + 3 * i0] = 0.0F; + fv2[i + 3 * i0] = 0.0F; for (i1 = 0; i1 < 12; i1++) { - fv1[i + 3 * i0] += (real32_T)iv3[i + 3 * i1] * P_apriori[i1 + 12 * + fv2[i + 3 * i0] += (real32_T)iv3[i + 3 * i1] * P_apriori[i1 + 12 * i0]; } } @@ -451,46 +484,46 @@ void attitudeKalmanfilter(const uint8_T updateVect[3], real32_T dt, const for (i = 0; i < 3; i++) { for (i0 = 0; i0 < 3; i0++) { - y = 0.0F; + f0 = 0.0F; for (i1 = 0; i1 < 12; i1++) { - y += fv1[i + 3 * i1] * (real32_T)iv2[i1 + 12 * i0]; + f0 += fv2[i + 3 * i1] * (real32_T)iv2[i1 + 12 * i0]; } - c_a[i + 3 * i0] = y + O[i + 3 * i0]; + a[i + 3 * i0] = f0 + O[i + 3 * i0]; } } - b_mrdivide(c_P_apriori, c_a, S_k); + b_mrdivide(c_P_apriori, a, S_k); - /* 'attitudeKalmanfilter:121' x_aposteriori=x_apriori+K_k*y_k; */ + /* 'attitudeKalmanfilter:131' x_aposteriori=x_apriori+K_k*y_k; */ for (i = 0; i < 3; i++) { - y = 0.0F; + f0 = 0.0F; for (i0 = 0; i0 < 12; i0++) { - y += (real32_T)iv3[i + 3 * i0] * x_apriori[i0]; + f0 += (real32_T)iv3[i + 3 * i0] * x_apriori[i0]; } - x_n_b[i] = z[i] - y; + x_n_b[i] = z[i] - f0; } for (i = 0; i < 12; i++) { - y = 0.0F; + f0 = 0.0F; for (i0 = 0; i0 < 3; i0++) { - y += S_k[i + 12 * i0] * x_n_b[i0]; + f0 += S_k[i + 12 * i0] * x_n_b[i0]; } - x_aposteriori[i] = x_apriori[i] + y; + x_aposteriori[i] = x_apriori[i] + f0; } - /* 'attitudeKalmanfilter:122' P_aposteriori=(eye(12)-K_k*H_k(1:3,1:12))*P_apriori; */ + /* 'attitudeKalmanfilter:132' P_aposteriori=(eye(12)-K_k*H_k(1:3,1:12))*P_apriori; */ b_eye(dv1); for (i = 0; i < 12; i++) { for (i0 = 0; i0 < 12; i0++) { - y = 0.0F; + f0 = 0.0F; for (i1 = 0; i1 < 3; i1++) { - y += S_k[i + 12 * i1] * (real32_T)iv3[i1 + 3 * i0]; + f0 += S_k[i + 12 * i1] * (real32_T)iv3[i1 + 3 * i0]; } - Q[i + 12 * i0] = (real32_T)dv1[i + 12 * i0] - y; + b_A_lin[i + 12 * i0] = (real32_T)dv1[i + 12 * i0] - f0; } } @@ -498,25 +531,25 @@ void attitudeKalmanfilter(const uint8_T updateVect[3], real32_T dt, const for (i0 = 0; i0 < 12; i0++) { P_aposteriori[i + 12 * i0] = 0.0F; for (i1 = 0; i1 < 12; i1++) { - P_aposteriori[i + 12 * i0] += Q[i + 12 * i1] * P_apriori[i1 + 12 * - i0]; + P_aposteriori[i + 12 * i0] += b_A_lin[i + 12 * i1] * P_apriori[i1 + + 12 * i0]; } } } } else { - /* 'attitudeKalmanfilter:123' else */ - /* 'attitudeKalmanfilter:124' if updateVect(1)==1&&updateVect(2)==1&&updateVect(3)==0 */ + /* 'attitudeKalmanfilter:133' else */ + /* 'attitudeKalmanfilter:134' if updateVect(1)==1&&updateVect(2)==1&&updateVect(3)==0 */ if ((updateVect[0] == 1) && (updateVect[1] == 1) && (updateVect[2] == 0)) { - /* 'attitudeKalmanfilter:125' R=diag(r(1:6)); */ - d_diag(*(real32_T (*)[6])&r[0], S_k); + /* 'attitudeKalmanfilter:135' R=diag(r(1:6)); */ + c_diag(*(real32_T (*)[6])&r[0], S_k); /* observation matrix */ - /* 'attitudeKalmanfilter:128' H_k=[ E, E, Z, Z; */ - /* 'attitudeKalmanfilter:129' Z, Z, E, Z]; */ - /* 'attitudeKalmanfilter:131' y_k=z(1:6)-H_k(1:6,1:12)*x_apriori; */ - /* 'attitudeKalmanfilter:133' S_k=H_k(1:6,1:12)*P_apriori*H_k(1:6,1:12)'+R(1:6,1:6); */ - /* 'attitudeKalmanfilter:134' K_k=(P_apriori*H_k(1:6,1:12)'/(S_k)); */ + /* 'attitudeKalmanfilter:138' H_k=[ E, Es, Z, Z; */ + /* 'attitudeKalmanfilter:139' Z, Z, E, Z]; */ + /* 'attitudeKalmanfilter:141' y_k=z(1:6)-H_k(1:6,1:12)*x_apriori; */ + /* 'attitudeKalmanfilter:143' S_k=H_k(1:6,1:12)*P_apriori*H_k(1:6,1:12)'+R(1:6,1:6); */ + /* 'attitudeKalmanfilter:144' K_k=(P_apriori*H_k(1:6,1:12)'/(S_k)); */ for (i = 0; i < 12; i++) { for (i0 = 0; i0 < 6; i0++) { d_P_apriori[i + 12 * i0] = 0.0F; @@ -539,46 +572,46 @@ void attitudeKalmanfilter(const uint8_T updateVect[3], real32_T dt, const for (i = 0; i < 6; i++) { for (i0 = 0; i0 < 6; i0++) { - y = 0.0F; + f0 = 0.0F; for (i1 = 0; i1 < 12; i1++) { - y += b_K_k[i + 6 * i1] * (real32_T)iv4[i1 + 12 * i0]; + f0 += b_K_k[i + 6 * i1] * (real32_T)iv4[i1 + 12 * i0]; } - fv1[i + 6 * i0] = y + S_k[i + 6 * i0]; + fv2[i + 6 * i0] = f0 + S_k[i + 6 * i0]; } } - c_mrdivide(d_P_apriori, fv1, b_K_k); + c_mrdivide(d_P_apriori, fv2, b_K_k); - /* 'attitudeKalmanfilter:137' x_aposteriori=x_apriori+K_k*y_k; */ + /* 'attitudeKalmanfilter:147' x_aposteriori=x_apriori+K_k*y_k; */ for (i = 0; i < 6; i++) { - y = 0.0F; + f0 = 0.0F; for (i0 = 0; i0 < 12; i0++) { - y += (real32_T)iv5[i + 6 * i0] * x_apriori[i0]; + f0 += (real32_T)iv5[i + 6 * i0] * x_apriori[i0]; } - b_r[i] = z[i] - y; + b_r[i] = z[i] - f0; } for (i = 0; i < 12; i++) { - y = 0.0F; + f0 = 0.0F; for (i0 = 0; i0 < 6; i0++) { - y += b_K_k[i + 12 * i0] * b_r[i0]; + f0 += b_K_k[i + 12 * i0] * b_r[i0]; } - x_aposteriori[i] = x_apriori[i] + y; + x_aposteriori[i] = x_apriori[i] + f0; } - /* 'attitudeKalmanfilter:138' P_aposteriori=(eye(12)-K_k*H_k(1:6,1:12))*P_apriori; */ + /* 'attitudeKalmanfilter:148' P_aposteriori=(eye(12)-K_k*H_k(1:6,1:12))*P_apriori; */ b_eye(dv1); for (i = 0; i < 12; i++) { for (i0 = 0; i0 < 12; i0++) { - y = 0.0F; + f0 = 0.0F; for (i1 = 0; i1 < 6; i1++) { - y += b_K_k[i + 12 * i1] * (real32_T)iv5[i1 + 6 * i0]; + f0 += b_K_k[i + 12 * i1] * (real32_T)iv5[i1 + 6 * i0]; } - Q[i + 12 * i0] = (real32_T)dv1[i + 12 * i0] - y; + b_A_lin[i + 12 * i0] = (real32_T)dv1[i + 12 * i0] - f0; } } @@ -586,22 +619,22 @@ void attitudeKalmanfilter(const uint8_T updateVect[3], real32_T dt, const for (i0 = 0; i0 < 12; i0++) { P_aposteriori[i + 12 * i0] = 0.0F; for (i1 = 0; i1 < 12; i1++) { - P_aposteriori[i + 12 * i0] += Q[i + 12 * i1] * P_apriori[i1 + 12 * - i0]; + P_aposteriori[i + 12 * i0] += b_A_lin[i + 12 * i1] * P_apriori[i1 + + 12 * i0]; } } } } else { - /* 'attitudeKalmanfilter:139' else */ - /* 'attitudeKalmanfilter:140' if updateVect(1)==1&&updateVect(2)==0&&updateVect(3)==1 */ + /* 'attitudeKalmanfilter:149' else */ + /* 'attitudeKalmanfilter:150' if updateVect(1)==1&&updateVect(2)==0&&updateVect(3)==1 */ if ((updateVect[0] == 1) && (updateVect[1] == 0) && (updateVect[2] == 1)) { - /* 'attitudeKalmanfilter:141' R=diag([r(1:3);r(7:9)]); */ + /* 'attitudeKalmanfilter:151' R=diag([r(1:3);r(7:9)]); */ /* observation matrix */ - /* 'attitudeKalmanfilter:144' H_k=[ E, E, Z, Z; */ - /* 'attitudeKalmanfilter:145' Z, Z, Z, E]; */ - /* 'attitudeKalmanfilter:147' y_k=[z(1:3);z(7:9)]-H_k(1:6,1:12)*x_apriori; */ - /* 'attitudeKalmanfilter:149' S_k=H_k(1:6,1:12)*P_apriori*H_k(1:6,1:12)'+R(1:6,1:6); */ + /* 'attitudeKalmanfilter:154' H_k=[ E, Es, Z, Z; */ + /* 'attitudeKalmanfilter:155' Z, Z, Z, E]; */ + /* 'attitudeKalmanfilter:157' y_k=[z(1:3);z(7:9)]-H_k(1:6,1:12)*x_apriori; */ + /* 'attitudeKalmanfilter:159' S_k=H_k(1:6,1:12)*P_apriori*H_k(1:6,1:12)'+R(1:6,1:6); */ for (i = 0; i < 6; i++) { for (i0 = 0; i0 < 12; i0++) { b_K_k[i + 6 * i0] = 0.0F; @@ -619,16 +652,16 @@ void attitudeKalmanfilter(const uint8_T updateVect[3], real32_T dt, const for (i = 0; i < 6; i++) { for (i0 = 0; i0 < 6; i0++) { - y = 0.0F; + f0 = 0.0F; for (i1 = 0; i1 < 12; i1++) { - y += b_K_k[i + 6 * i1] * (real32_T)iv7[i1 + 12 * i0]; + f0 += b_K_k[i + 6 * i1] * (real32_T)iv7[i1 + 12 * i0]; } - S_k[i + 6 * i0] = y + b_r[3 * (i + i0)]; + S_k[i + 6 * i0] = f0 + b_r[3 * (i + i0)]; } } - /* 'attitudeKalmanfilter:150' K_k=(P_apriori*H_k(1:6,1:12)'/(S_k)); */ + /* 'attitudeKalmanfilter:160' K_k=(P_apriori*H_k(1:6,1:12)'/(S_k)); */ for (i = 0; i < 12; i++) { for (i0 = 0; i0 < 6; i0++) { d_P_apriori[i + 12 * i0] = 0.0F; @@ -641,7 +674,7 @@ void attitudeKalmanfilter(const uint8_T updateVect[3], real32_T dt, const c_mrdivide(d_P_apriori, S_k, b_K_k); - /* 'attitudeKalmanfilter:153' x_aposteriori=x_apriori+K_k*y_k; */ + /* 'attitudeKalmanfilter:163' x_aposteriori=x_apriori+K_k*y_k; */ for (i = 0; i < 3; i++) { b_r[i] = z[i]; } @@ -651,33 +684,33 @@ void attitudeKalmanfilter(const uint8_T updateVect[3], real32_T dt, const } for (i = 0; i < 6; i++) { - fv2[i] = 0.0F; + fv3[i] = 0.0F; for (i0 = 0; i0 < 12; i0++) { - fv2[i] += (real32_T)iv6[i + 6 * i0] * x_apriori[i0]; + fv3[i] += (real32_T)iv6[i + 6 * i0] * x_apriori[i0]; } - b_z[i] = b_r[i] - fv2[i]; + b_z[i] = b_r[i] - fv3[i]; } for (i = 0; i < 12; i++) { - y = 0.0F; + f0 = 0.0F; for (i0 = 0; i0 < 6; i0++) { - y += b_K_k[i + 12 * i0] * b_z[i0]; + f0 += b_K_k[i + 12 * i0] * b_z[i0]; } - x_aposteriori[i] = x_apriori[i] + y; + x_aposteriori[i] = x_apriori[i] + f0; } - /* 'attitudeKalmanfilter:154' P_aposteriori=(eye(12)-K_k*H_k(1:6,1:12))*P_apriori; */ + /* 'attitudeKalmanfilter:164' P_aposteriori=(eye(12)-K_k*H_k(1:6,1:12))*P_apriori; */ b_eye(dv1); for (i = 0; i < 12; i++) { for (i0 = 0; i0 < 12; i0++) { - y = 0.0F; + f0 = 0.0F; for (i1 = 0; i1 < 6; i1++) { - y += b_K_k[i + 12 * i1] * (real32_T)iv6[i1 + 6 * i0]; + f0 += b_K_k[i + 12 * i1] * (real32_T)iv6[i1 + 6 * i0]; } - Q[i + 12 * i0] = (real32_T)dv1[i + 12 * i0] - y; + b_A_lin[i + 12 * i0] = (real32_T)dv1[i + 12 * i0] - f0; } } @@ -685,66 +718,74 @@ void attitudeKalmanfilter(const uint8_T updateVect[3], real32_T dt, const for (i0 = 0; i0 < 12; i0++) { P_aposteriori[i + 12 * i0] = 0.0F; for (i1 = 0; i1 < 12; i1++) { - P_aposteriori[i + 12 * i0] += Q[i + 12 * i1] * P_apriori[i1 + 12 - * i0]; + P_aposteriori[i + 12 * i0] += b_A_lin[i + 12 * i1] * + P_apriori[i1 + 12 * i0]; } } } } else { - /* 'attitudeKalmanfilter:155' else */ - /* 'attitudeKalmanfilter:156' x_aposteriori=x_apriori; */ + /* 'attitudeKalmanfilter:165' else */ + /* 'attitudeKalmanfilter:166' x_aposteriori=x_apriori; */ for (i = 0; i < 12; i++) { x_aposteriori[i] = x_apriori[i]; } - /* 'attitudeKalmanfilter:157' P_aposteriori=P_apriori; */ - memcpy((void *)&P_aposteriori[0], (void *)&P_apriori[0], 144U * sizeof - (real32_T)); + /* 'attitudeKalmanfilter:167' P_aposteriori=P_apriori; */ + memcpy(&P_aposteriori[0], &P_apriori[0], 144U * sizeof(real32_T)); } } } } /* % euler anglels extraction */ - /* 'attitudeKalmanfilter:166' z_n_b = -x_aposteriori(7:9)./norm(x_aposteriori(7:9)); */ - y = norm(*(real32_T (*)[3])&x_aposteriori[6]); + /* 'attitudeKalmanfilter:176' z_n_b = -x_aposteriori(7:9)./norm(x_aposteriori(7:9)); */ + for (i = 0; i < 3; i++) { + x_n_b[i] = -x_aposteriori[i + 6]; + } - /* 'attitudeKalmanfilter:167' m_n_b = x_aposteriori(10:12)./norm(x_aposteriori(10:12)); */ - b_y = norm(*(real32_T (*)[3])&x_aposteriori[9]); + rdivide(x_n_b, norm(*(real32_T (*)[3])&x_aposteriori[6]), z_n_b); - /* 'attitudeKalmanfilter:169' y_n_b=cross(z_n_b,m_n_b); */ + /* 'attitudeKalmanfilter:177' m_n_b = x_aposteriori(10:12)./norm(x_aposteriori(10:12)); */ + rdivide(*(real32_T (*)[3])&x_aposteriori[9], norm(*(real32_T (*)[3])& + x_aposteriori[9]), m_n_b); + + /* 'attitudeKalmanfilter:179' y_n_b=cross(z_n_b,m_n_b); */ + for (i = 0; i < 3; i++) { + x_n_b[i] = m_n_b[i]; + } + + cross(z_n_b, x_n_b, m_n_b); + + /* 'attitudeKalmanfilter:180' y_n_b=y_n_b./norm(y_n_b); */ for (i = 0; i < 3; i++) { - z_n_b[i] = -x_aposteriori[i + 6] / y; - x_n_b[i] = x_aposteriori[i + 9] / b_y; + x_n_b[i] = m_n_b[i]; } - cross(z_n_b, x_n_b, y_n_b); + rdivide(x_n_b, norm(m_n_b), m_n_b); + + /* 'attitudeKalmanfilter:182' x_n_b=(cross(y_n_b,z_n_b)); */ + cross(m_n_b, z_n_b, x_n_b); - /* 'attitudeKalmanfilter:170' y_n_b=y_n_b./norm(y_n_b); */ - y = norm(y_n_b); + /* 'attitudeKalmanfilter:183' x_n_b=x_n_b./norm(x_n_b); */ for (i = 0; i < 3; i++) { - y_n_b[i] /= y; + b_x_aposteriori_k[i] = x_n_b[i]; } - /* 'attitudeKalmanfilter:172' x_n_b=(cross(y_n_b,z_n_b)); */ - cross(y_n_b, z_n_b, x_n_b); + rdivide(b_x_aposteriori_k, norm(x_n_b), x_n_b); - /* 'attitudeKalmanfilter:173' x_n_b=x_n_b./norm(x_n_b); */ - y = norm(x_n_b); + /* 'attitudeKalmanfilter:189' Rot_matrix=[x_n_b,y_n_b,z_n_b]; */ for (i = 0; i < 3; i++) { - /* 'attitudeKalmanfilter:179' Rot_matrix=[x_n_b,y_n_b,z_n_b]; */ - Rot_matrix[i] = x_n_b[i] / y; - Rot_matrix[3 + i] = y_n_b[i]; + Rot_matrix[i] = x_n_b[i]; + Rot_matrix[3 + i] = m_n_b[i]; Rot_matrix[6 + i] = z_n_b[i]; } - /* 'attitudeKalmanfilter:183' phi=atan2(Rot_matrix(2,3),Rot_matrix(3,3)); */ - /* 'attitudeKalmanfilter:184' theta=-asin(Rot_matrix(1,3)); */ - /* 'attitudeKalmanfilter:185' psi=atan2(Rot_matrix(1,2),Rot_matrix(1,1)); */ - /* 'attitudeKalmanfilter:186' eulerAngles=[phi;theta;psi]; */ + /* 'attitudeKalmanfilter:193' phi=atan2(Rot_matrix(2,3),Rot_matrix(3,3)); */ + /* 'attitudeKalmanfilter:194' theta=-asin(Rot_matrix(1,3)); */ + /* 'attitudeKalmanfilter:195' psi=atan2(Rot_matrix(1,2),Rot_matrix(1,1)); */ + /* 'attitudeKalmanfilter:196' eulerAngles=[phi;theta;psi]; */ eulerAngles[0] = rt_atan2f_snf(Rot_matrix[7], Rot_matrix[8]); eulerAngles[1] = -(real32_T)asinf(Rot_matrix[6]); eulerAngles[2] = rt_atan2f_snf(Rot_matrix[3], Rot_matrix[0]); } -/* End of code generation (attitudeKalmanfilter.c) */ diff --git a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter.h b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter.h index f8f99fa5a..9d89705a6 100755 --- a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter.h +++ b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter.h @@ -3,7 +3,7 @@ * * Code generation for function 'attitudeKalmanfilter' * - * C source code generated on: Mon Oct 01 19:38:49 2012 + * C source code generated on: Sat Oct 13 16:28:18 2012 * */ @@ -11,7 +11,7 @@ #define __ATTITUDEKALMANFILTER_H__ /* Include files */ #include -#include +#include #include #include #include "rt_defines.h" diff --git a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.c b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.c index 689bc49e9..27eb2763e 100755 --- a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.c +++ b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.c @@ -3,7 +3,7 @@ * * Code generation for function 'attitudeKalmanfilter_initialize' * - * C source code generated on: Mon Oct 01 19:38:49 2012 + * C source code generated on: Sat Oct 13 16:28:18 2012 * */ diff --git a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.h b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.h index dcce3cd72..277df273d 100755 --- a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.h +++ b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.h @@ -3,7 +3,7 @@ * * Code generation for function 'attitudeKalmanfilter_initialize' * - * C source code generated on: Mon Oct 01 19:38:49 2012 + * C source code generated on: Sat Oct 13 16:28:18 2012 * */ @@ -11,7 +11,7 @@ #define __ATTITUDEKALMANFILTER_INITIALIZE_H__ /* Include files */ #include -#include +#include #include #include #include "rt_defines.h" diff --git a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.c b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.c index 39f8f648c..ed0993039 100755 --- a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.c +++ b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.c @@ -3,7 +3,7 @@ * * Code generation for function 'attitudeKalmanfilter_terminate' * - * C source code generated on: Mon Oct 01 19:38:49 2012 + * C source code generated on: Sat Oct 13 16:28:18 2012 * */ diff --git a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.h b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.h index ea7b9e03e..0b0ccd073 100755 --- a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.h +++ b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.h @@ -3,7 +3,7 @@ * * Code generation for function 'attitudeKalmanfilter_terminate' * - * C source code generated on: Mon Oct 01 19:38:49 2012 + * C source code generated on: Sat Oct 13 16:28:18 2012 * */ @@ -11,7 +11,7 @@ #define __ATTITUDEKALMANFILTER_TERMINATE_H__ /* Include files */ #include -#include +#include #include #include #include "rt_defines.h" diff --git a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_types.h b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_types.h index 6d5704a7a..95814863f 100755 --- a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_types.h +++ b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_types.h @@ -3,7 +3,7 @@ * * Code generation for function 'attitudeKalmanfilter' * - * C source code generated on: Mon Oct 01 19:38:49 2012 + * C source code generated on: Sat Oct 13 16:28:18 2012 * */ diff --git a/apps/attitude_estimator_ekf/codegen/cross.c b/apps/attitude_estimator_ekf/codegen/cross.c index 27da4b6b9..d4524fed8 100755 --- a/apps/attitude_estimator_ekf/codegen/cross.c +++ b/apps/attitude_estimator_ekf/codegen/cross.c @@ -3,7 +3,7 @@ * * Code generation for function 'cross' * - * C source code generated on: Mon Oct 01 19:38:49 2012 + * C source code generated on: Sat Oct 13 16:28:18 2012 * */ diff --git a/apps/attitude_estimator_ekf/codegen/cross.h b/apps/attitude_estimator_ekf/codegen/cross.h index 8ef2c475c..d70b0adc2 100755 --- a/apps/attitude_estimator_ekf/codegen/cross.h +++ b/apps/attitude_estimator_ekf/codegen/cross.h @@ -3,7 +3,7 @@ * * Code generation for function 'cross' * - * C source code generated on: Mon Oct 01 19:38:49 2012 + * C source code generated on: Sat Oct 13 16:28:18 2012 * */ @@ -11,7 +11,7 @@ #define __CROSS_H__ /* Include files */ #include -#include +#include #include #include #include "rt_defines.h" diff --git a/apps/attitude_estimator_ekf/codegen/diag.c b/apps/attitude_estimator_ekf/codegen/diag.c index 81626589d..58e9f553f 100755 --- a/apps/attitude_estimator_ekf/codegen/diag.c +++ b/apps/attitude_estimator_ekf/codegen/diag.c @@ -3,7 +3,7 @@ * * Code generation for function 'diag' * - * C source code generated on: Mon Oct 01 19:38:49 2012 + * C source code generated on: Sat Oct 13 16:28:18 2012 * */ @@ -27,19 +27,7 @@ /* * */ -void b_diag(const real32_T v[9], real32_T d[81]) -{ - int32_T j; - memset((void *)&d[0], 0, 81U * sizeof(real32_T)); - for (j = 0; j < 9; j++) { - d[j + 9 * j] = v[j]; - } -} - -/* - * - */ -void c_diag(const real32_T v[3], real32_T d[9]) +void b_diag(const real32_T v[3], real32_T d[9]) { int32_T j; for (j = 0; j < 9; j++) { @@ -54,10 +42,10 @@ void c_diag(const real32_T v[3], real32_T d[9]) /* * */ -void d_diag(const real32_T v[6], real32_T d[36]) +void c_diag(const real32_T v[6], real32_T d[36]) { int32_T j; - memset((void *)&d[0], 0, 36U * sizeof(real32_T)); + memset(&d[0], 0, 36U * sizeof(real32_T)); for (j = 0; j < 6; j++) { d[j + 6 * j] = v[j]; } @@ -66,12 +54,12 @@ void d_diag(const real32_T v[6], real32_T d[36]) /* * */ -void diag(const real32_T v[12], real32_T d[144]) +void diag(const real32_T v[9], real32_T d[81]) { int32_T j; - memset((void *)&d[0], 0, 144U * sizeof(real32_T)); - for (j = 0; j < 12; j++) { - d[j + 12 * j] = v[j]; + memset(&d[0], 0, 81U * sizeof(real32_T)); + for (j = 0; j < 9; j++) { + d[j + 9 * j] = v[j]; } } diff --git a/apps/attitude_estimator_ekf/codegen/diag.h b/apps/attitude_estimator_ekf/codegen/diag.h index 10cea81b2..85ff0e0e9 100755 --- a/apps/attitude_estimator_ekf/codegen/diag.h +++ b/apps/attitude_estimator_ekf/codegen/diag.h @@ -3,7 +3,7 @@ * * Code generation for function 'diag' * - * C source code generated on: Mon Oct 01 19:38:49 2012 + * C source code generated on: Sat Oct 13 16:28:18 2012 * */ @@ -11,7 +11,7 @@ #define __DIAG_H__ /* Include files */ #include -#include +#include #include #include #include "rt_defines.h" @@ -29,9 +29,8 @@ /* Variable Definitions */ /* Function Declarations */ -extern void b_diag(const real32_T v[9], real32_T d[81]); -extern void c_diag(const real32_T v[3], real32_T d[9]); -extern void d_diag(const real32_T v[6], real32_T d[36]); -extern void diag(const real32_T v[12], real32_T d[144]); +extern void b_diag(const real32_T v[3], real32_T d[9]); +extern void c_diag(const real32_T v[6], real32_T d[36]); +extern void diag(const real32_T v[9], real32_T d[81]); #endif /* End of code generation (diag.h) */ diff --git a/apps/attitude_estimator_ekf/codegen/eye.c b/apps/attitude_estimator_ekf/codegen/eye.c index 2db070e6f..d7ae20afd 100755 --- a/apps/attitude_estimator_ekf/codegen/eye.c +++ b/apps/attitude_estimator_ekf/codegen/eye.c @@ -3,7 +3,7 @@ * * Code generation for function 'eye' * - * C source code generated on: Mon Oct 01 19:38:49 2012 + * C source code generated on: Sat Oct 13 16:28:18 2012 * */ @@ -30,7 +30,7 @@ void b_eye(real_T I[144]) { int32_T i; - memset((void *)&I[0], 0, 144U * sizeof(real_T)); + memset(&I[0], 0, 144U * sizeof(real_T)); for (i = 0; i < 12; i++) { I[i + 12 * i] = 1.0; } @@ -42,7 +42,7 @@ void b_eye(real_T I[144]) void eye(real_T I[9]) { int32_T i; - memset((void *)&I[0], 0, 9U * sizeof(real_T)); + memset(&I[0], 0, 9U * sizeof(real_T)); for (i = 0; i < 3; i++) { I[i + 3 * i] = 1.0; } diff --git a/apps/attitude_estimator_ekf/codegen/eye.h b/apps/attitude_estimator_ekf/codegen/eye.h index 027db1c06..77099ec22 100755 --- a/apps/attitude_estimator_ekf/codegen/eye.h +++ b/apps/attitude_estimator_ekf/codegen/eye.h @@ -3,7 +3,7 @@ * * Code generation for function 'eye' * - * C source code generated on: Mon Oct 01 19:38:49 2012 + * C source code generated on: Sat Oct 13 16:28:18 2012 * */ @@ -11,7 +11,7 @@ #define __EYE_H__ /* Include files */ #include -#include +#include #include #include #include "rt_defines.h" diff --git a/apps/attitude_estimator_ekf/codegen/mrdivide.c b/apps/attitude_estimator_ekf/codegen/mrdivide.c index ce29e42cd..1c69f1ef7 100755 --- a/apps/attitude_estimator_ekf/codegen/mrdivide.c +++ b/apps/attitude_estimator_ekf/codegen/mrdivide.c @@ -3,7 +3,7 @@ * * Code generation for function 'mrdivide' * - * C source code generated on: Mon Oct 01 19:38:49 2012 + * C source code generated on: Sat Oct 13 16:28:18 2012 * */ @@ -29,9 +29,9 @@ */ void b_mrdivide(const real32_T A[36], const real32_T B[9], real32_T y[36]) { + real32_T b_A[9]; int32_T rtemp; int32_T k; - real32_T b_A[9]; real32_T b_B[36]; int32_T r1; int32_T r2; @@ -54,15 +54,15 @@ void b_mrdivide(const real32_T A[36], const real32_T B[9], real32_T y[36]) r1 = 0; r2 = 1; r3 = 2; - maxval = (real32_T)fabsf(b_A[0]); - a21 = (real32_T)fabsf(b_A[1]); + maxval = (real32_T)fabs(b_A[0]); + a21 = (real32_T)fabs(b_A[1]); if (a21 > maxval) { maxval = a21; r1 = 1; r2 = 0; } - if ((real32_T)fabsf(b_A[2]) > maxval) { + if ((real32_T)fabs(b_A[2]) > maxval) { r1 = 2; r2 = 1; r3 = 0; @@ -74,7 +74,7 @@ void b_mrdivide(const real32_T A[36], const real32_T B[9], real32_T y[36]) b_A[3 + r3] -= b_A[r3] * b_A[3 + r1]; b_A[6 + r2] -= b_A[r2] * b_A[6 + r1]; b_A[6 + r3] -= b_A[r3] * b_A[6 + r1]; - if ((real32_T)fabsf(b_A[3 + r3]) > (real32_T)fabsf(b_A[3 + r2])) { + if ((real32_T)fabs(b_A[3 + r3]) > (real32_T)fabs(b_A[3 + r2])) { rtemp = r2; r2 = r3; r3 = rtemp; @@ -107,51 +107,46 @@ void b_mrdivide(const real32_T A[36], const real32_T B[9], real32_T y[36]) */ void c_mrdivide(const real32_T A[72], const real32_T B[36], real32_T y[72]) { - int32_T jy; - int32_T iy; real32_T b_A[36]; int8_T ipiv[6]; + int32_T i3; + int32_T iy; int32_T j; - int32_T mmj; - int32_T jj; - int32_T jp1j; int32_T c; int32_T ix; real32_T temp; int32_T k; real32_T s; - int32_T loop_ub; + int32_T jy; + int32_T ijA; real32_T Y[72]; - for (jy = 0; jy < 6; jy++) { + for (i3 = 0; i3 < 6; i3++) { for (iy = 0; iy < 6; iy++) { - b_A[iy + 6 * jy] = B[jy + 6 * iy]; + b_A[iy + 6 * i3] = B[i3 + 6 * iy]; } - ipiv[jy] = (int8_T)(1 + jy); + ipiv[i3] = (int8_T)(1 + i3); } for (j = 0; j < 5; j++) { - mmj = -j; - jj = j * 7; - jp1j = jj + 1; - c = mmj + 6; - jy = 0; - ix = jj; - temp = (real32_T)fabsf(b_A[jj]); - for (k = 2; k <= c; k++) { + c = j * 7; + iy = 0; + ix = c; + temp = (real32_T)fabs(b_A[c]); + for (k = 2; k <= 6 - j; k++) { ix++; - s = (real32_T)fabsf(b_A[ix]); + s = (real32_T)fabs(b_A[ix]); if (s > temp) { - jy = k - 1; + iy = k - 1; temp = s; } } - if ((real_T)b_A[jj + jy] != 0.0) { - if (jy != 0) { - ipiv[j] = (int8_T)((j + jy) + 1); + if (b_A[c + iy] != 0.0F) { + if (iy != 0) { + ipiv[j] = (int8_T)((j + iy) + 1); ix = j; - iy = j + jy; + iy += j; for (k = 0; k < 6; k++) { temp = b_A[ix]; b_A[ix] = b_A[iy]; @@ -161,42 +156,42 @@ void c_mrdivide(const real32_T A[72], const real32_T B[36], real32_T y[72]) } } - loop_ub = (jp1j + mmj) + 5; - for (iy = jp1j; iy + 1 <= loop_ub; iy++) { - b_A[iy] /= b_A[jj]; + i3 = (c - j) + 6; + for (jy = c + 1; jy + 1 <= i3; jy++) { + b_A[jy] /= b_A[c]; } } - c = 5 - j; - jy = jj + 6; - for (iy = 1; iy <= c; iy++) { - if ((real_T)b_A[jy] != 0.0) { - temp = b_A[jy] * -1.0F; - ix = jp1j; - loop_ub = (mmj + jj) + 12; - for (k = 7 + jj; k + 1 <= loop_ub; k++) { - b_A[k] += b_A[ix] * temp; + iy = c; + jy = c + 6; + for (k = 1; k <= 5 - j; k++) { + temp = b_A[jy]; + if (b_A[jy] != 0.0F) { + ix = c + 1; + i3 = (iy - j) + 12; + for (ijA = 7 + iy; ijA + 1 <= i3; ijA++) { + b_A[ijA] += b_A[ix] * -temp; ix++; } } jy += 6; - jj += 6; + iy += 6; } } - for (jy = 0; jy < 12; jy++) { + for (i3 = 0; i3 < 12; i3++) { for (iy = 0; iy < 6; iy++) { - Y[iy + 6 * jy] = A[jy + 12 * iy]; + Y[iy + 6 * i3] = A[i3 + 12 * iy]; } } - for (iy = 0; iy < 6; iy++) { - if (ipiv[iy] != iy + 1) { + for (jy = 0; jy < 6; jy++) { + if (ipiv[jy] != jy + 1) { for (j = 0; j < 12; j++) { - temp = Y[iy + 6 * j]; - Y[iy + 6 * j] = Y[(ipiv[iy] + 6 * j) - 1]; - Y[(ipiv[iy] + 6 * j) - 1] = temp; + temp = Y[jy + 6 * j]; + Y[jy + 6 * j] = Y[(ipiv[jy] + 6 * j) - 1]; + Y[(ipiv[jy] + 6 * j) - 1] = temp; } } } @@ -204,10 +199,10 @@ void c_mrdivide(const real32_T A[72], const real32_T B[36], real32_T y[72]) for (j = 0; j < 12; j++) { c = 6 * j; for (k = 0; k < 6; k++) { - jy = 6 * k; - if ((real_T)Y[k + c] != 0.0) { - for (iy = k + 2; iy < 7; iy++) { - Y[(iy + c) - 1] -= Y[k + c] * b_A[(iy + jy) - 1]; + iy = 6 * k; + if (Y[k + c] != 0.0F) { + for (jy = k + 2; jy < 7; jy++) { + Y[(jy + c) - 1] -= Y[k + c] * b_A[(jy + iy) - 1]; } } } @@ -216,19 +211,19 @@ void c_mrdivide(const real32_T A[72], const real32_T B[36], real32_T y[72]) for (j = 0; j < 12; j++) { c = 6 * j; for (k = 5; k > -1; k += -1) { - jy = 6 * k; - if ((real_T)Y[k + c] != 0.0) { - Y[k + c] /= b_A[k + jy]; - for (iy = 0; iy + 1 <= k; iy++) { - Y[iy + c] -= Y[k + c] * b_A[iy + jy]; + iy = 6 * k; + if (Y[k + c] != 0.0F) { + Y[k + c] /= b_A[k + iy]; + for (jy = 0; jy + 1 <= k; jy++) { + Y[jy + c] -= Y[k + c] * b_A[jy + iy]; } } } } - for (jy = 0; jy < 6; jy++) { + for (i3 = 0; i3 < 6; i3++) { for (iy = 0; iy < 12; iy++) { - y[iy + 12 * jy] = Y[jy + 6 * iy]; + y[iy + 12 * i3] = Y[i3 + 6 * iy]; } } } @@ -238,51 +233,46 @@ void c_mrdivide(const real32_T A[72], const real32_T B[36], real32_T y[72]) */ void mrdivide(const real32_T A[108], const real32_T B[81], real32_T y[108]) { - int32_T jy; - int32_T iy; real32_T b_A[81]; int8_T ipiv[9]; + int32_T i2; + int32_T iy; int32_T j; - int32_T mmj; - int32_T jj; - int32_T jp1j; int32_T c; int32_T ix; real32_T temp; int32_T k; real32_T s; - int32_T loop_ub; + int32_T jy; + int32_T ijA; real32_T Y[108]; - for (jy = 0; jy < 9; jy++) { + for (i2 = 0; i2 < 9; i2++) { for (iy = 0; iy < 9; iy++) { - b_A[iy + 9 * jy] = B[jy + 9 * iy]; + b_A[iy + 9 * i2] = B[i2 + 9 * iy]; } - ipiv[jy] = (int8_T)(1 + jy); + ipiv[i2] = (int8_T)(1 + i2); } for (j = 0; j < 8; j++) { - mmj = -j; - jj = j * 10; - jp1j = jj + 1; - c = mmj + 9; - jy = 0; - ix = jj; - temp = (real32_T)fabsf(b_A[jj]); - for (k = 2; k <= c; k++) { + c = j * 10; + iy = 0; + ix = c; + temp = (real32_T)fabs(b_A[c]); + for (k = 2; k <= 9 - j; k++) { ix++; - s = (real32_T)fabsf(b_A[ix]); + s = (real32_T)fabs(b_A[ix]); if (s > temp) { - jy = k - 1; + iy = k - 1; temp = s; } } - if ((real_T)b_A[jj + jy] != 0.0) { - if (jy != 0) { - ipiv[j] = (int8_T)((j + jy) + 1); + if (b_A[c + iy] != 0.0F) { + if (iy != 0) { + ipiv[j] = (int8_T)((j + iy) + 1); ix = j; - iy = j + jy; + iy += j; for (k = 0; k < 9; k++) { temp = b_A[ix]; b_A[ix] = b_A[iy]; @@ -292,42 +282,42 @@ void mrdivide(const real32_T A[108], const real32_T B[81], real32_T y[108]) } } - loop_ub = (jp1j + mmj) + 8; - for (iy = jp1j; iy + 1 <= loop_ub; iy++) { - b_A[iy] /= b_A[jj]; + i2 = (c - j) + 9; + for (jy = c + 1; jy + 1 <= i2; jy++) { + b_A[jy] /= b_A[c]; } } - c = 8 - j; - jy = jj + 9; - for (iy = 1; iy <= c; iy++) { - if ((real_T)b_A[jy] != 0.0) { - temp = b_A[jy] * -1.0F; - ix = jp1j; - loop_ub = (mmj + jj) + 18; - for (k = 10 + jj; k + 1 <= loop_ub; k++) { - b_A[k] += b_A[ix] * temp; + iy = c; + jy = c + 9; + for (k = 1; k <= 8 - j; k++) { + temp = b_A[jy]; + if (b_A[jy] != 0.0F) { + ix = c + 1; + i2 = (iy - j) + 18; + for (ijA = 10 + iy; ijA + 1 <= i2; ijA++) { + b_A[ijA] += b_A[ix] * -temp; ix++; } } jy += 9; - jj += 9; + iy += 9; } } - for (jy = 0; jy < 12; jy++) { + for (i2 = 0; i2 < 12; i2++) { for (iy = 0; iy < 9; iy++) { - Y[iy + 9 * jy] = A[jy + 12 * iy]; + Y[iy + 9 * i2] = A[i2 + 12 * iy]; } } - for (iy = 0; iy < 9; iy++) { - if (ipiv[iy] != iy + 1) { + for (jy = 0; jy < 9; jy++) { + if (ipiv[jy] != jy + 1) { for (j = 0; j < 12; j++) { - temp = Y[iy + 9 * j]; - Y[iy + 9 * j] = Y[(ipiv[iy] + 9 * j) - 1]; - Y[(ipiv[iy] + 9 * j) - 1] = temp; + temp = Y[jy + 9 * j]; + Y[jy + 9 * j] = Y[(ipiv[jy] + 9 * j) - 1]; + Y[(ipiv[jy] + 9 * j) - 1] = temp; } } } @@ -335,10 +325,10 @@ void mrdivide(const real32_T A[108], const real32_T B[81], real32_T y[108]) for (j = 0; j < 12; j++) { c = 9 * j; for (k = 0; k < 9; k++) { - jy = 9 * k; - if ((real_T)Y[k + c] != 0.0) { - for (iy = k + 2; iy < 10; iy++) { - Y[(iy + c) - 1] -= Y[k + c] * b_A[(iy + jy) - 1]; + iy = 9 * k; + if (Y[k + c] != 0.0F) { + for (jy = k + 2; jy < 10; jy++) { + Y[(jy + c) - 1] -= Y[k + c] * b_A[(jy + iy) - 1]; } } } @@ -347,19 +337,19 @@ void mrdivide(const real32_T A[108], const real32_T B[81], real32_T y[108]) for (j = 0; j < 12; j++) { c = 9 * j; for (k = 8; k > -1; k += -1) { - jy = 9 * k; - if ((real_T)Y[k + c] != 0.0) { - Y[k + c] /= b_A[k + jy]; - for (iy = 0; iy + 1 <= k; iy++) { - Y[iy + c] -= Y[k + c] * b_A[iy + jy]; + iy = 9 * k; + if (Y[k + c] != 0.0F) { + Y[k + c] /= b_A[k + iy]; + for (jy = 0; jy + 1 <= k; jy++) { + Y[jy + c] -= Y[k + c] * b_A[jy + iy]; } } } } - for (jy = 0; jy < 9; jy++) { + for (i2 = 0; i2 < 9; i2++) { for (iy = 0; iy < 12; iy++) { - y[iy + 12 * jy] = Y[jy + 9 * iy]; + y[iy + 12 * i2] = Y[i2 + 9 * iy]; } } } diff --git a/apps/attitude_estimator_ekf/codegen/mrdivide.h b/apps/attitude_estimator_ekf/codegen/mrdivide.h index b80f34357..75a396878 100755 --- a/apps/attitude_estimator_ekf/codegen/mrdivide.h +++ b/apps/attitude_estimator_ekf/codegen/mrdivide.h @@ -3,7 +3,7 @@ * * Code generation for function 'mrdivide' * - * C source code generated on: Mon Oct 01 19:38:49 2012 + * C source code generated on: Sat Oct 13 16:28:18 2012 * */ @@ -11,7 +11,7 @@ #define __MRDIVIDE_H__ /* Include files */ #include -#include +#include #include #include #include "rt_defines.h" diff --git a/apps/attitude_estimator_ekf/codegen/norm.c b/apps/attitude_estimator_ekf/codegen/norm.c index 341c93022..fbd5d43e0 100755 --- a/apps/attitude_estimator_ekf/codegen/norm.c +++ b/apps/attitude_estimator_ekf/codegen/norm.c @@ -3,7 +3,7 @@ * * Code generation for function 'norm' * - * C source code generated on: Mon Oct 01 19:38:49 2012 + * C source code generated on: Sat Oct 13 16:28:18 2012 * */ @@ -31,32 +31,24 @@ real32_T norm(const real32_T x[3]) { real32_T y; real32_T scale; - boolean_T firstNonZero; int32_T k; real32_T absxk; real32_T t; y = 0.0F; - scale = 0.0F; - firstNonZero = TRUE; + scale = 1.17549435E-38F; for (k = 0; k < 3; k++) { - if (x[k] != 0.0F) { - absxk = (real32_T)fabsf(x[k]); - if (firstNonZero) { - scale = absxk; - y = 1.0F; - firstNonZero = FALSE; - } else if (scale < absxk) { - t = scale / absxk; - y = 1.0F + y * t * t; - scale = absxk; - } else { - t = absxk / scale; - y += t * t; - } + absxk = (real32_T)fabs(x[k]); + if (absxk > scale) { + t = scale / absxk; + y = 1.0F + y * t * t; + scale = absxk; + } else { + t = absxk / scale; + y += t * t; } } - return scale * (real32_T)sqrtf(y); + return scale * (real32_T)sqrt(y); } /* End of code generation (norm.c) */ diff --git a/apps/attitude_estimator_ekf/codegen/norm.h b/apps/attitude_estimator_ekf/codegen/norm.h index 0f58dbe69..a2e9d90d4 100755 --- a/apps/attitude_estimator_ekf/codegen/norm.h +++ b/apps/attitude_estimator_ekf/codegen/norm.h @@ -3,7 +3,7 @@ * * Code generation for function 'norm' * - * C source code generated on: Mon Oct 01 19:38:49 2012 + * C source code generated on: Sat Oct 13 16:28:18 2012 * */ @@ -11,7 +11,7 @@ #define __NORM_H__ /* Include files */ #include -#include +#include #include #include #include "rt_defines.h" diff --git a/apps/attitude_estimator_ekf/codegen/power.c b/apps/attitude_estimator_ekf/codegen/power.c deleted file mode 100755 index 8672c7a85..000000000 --- a/apps/attitude_estimator_ekf/codegen/power.c +++ /dev/null @@ -1,84 +0,0 @@ -/* - * power.c - * - * Code generation for function 'power' - * - * C source code generated on: Mon Oct 01 19:38:49 2012 - * - */ - -/* Include files */ -#include "rt_nonfinite.h" -#include "attitudeKalmanfilter.h" -#include "power.h" - -/* Type Definitions */ - -/* Named Constants */ - -/* Variable Declarations */ - -/* Variable Definitions */ - -/* Function Declarations */ -static real32_T rt_powf_snf(real32_T u0, real32_T u1); - -/* Function Definitions */ -static real32_T rt_powf_snf(real32_T u0, real32_T u1) -{ - real32_T y; - real32_T f0; - real32_T f1; - if (rtIsNaNF(u0) || rtIsNaNF(u1)) { - y = ((real32_T)rtNaN); - } else { - f0 = (real32_T)fabsf(u0); - f1 = (real32_T)fabsf(u1); - if (rtIsInfF(u1)) { - if (f0 == 1.0F) { - y = ((real32_T)rtNaN); - } else if (f0 > 1.0F) { - if (u1 > 0.0F) { - y = ((real32_T)rtInf); - } else { - y = 0.0F; - } - } else if (u1 > 0.0F) { - y = 0.0F; - } else { - y = ((real32_T)rtInf); - } - } else if (f1 == 0.0F) { - y = 1.0F; - } else if (f1 == 1.0F) { - if (u1 > 0.0F) { - y = u0; - } else { - y = 1.0F / u0; - } - } else if (u1 == 2.0F) { - y = u0 * u0; - } else if ((u1 == 0.5F) && (u0 >= 0.0F)) { - y = (real32_T)sqrtf(u0); - } else if ((u0 < 0.0F) && (u1 > (real32_T)floorf(u1))) { - y = ((real32_T)rtNaN); - } else { - y = (real32_T)powf(u0, u1); - } - } - - return y; -} - -/* - * - */ -void power(const real32_T a[12], real_T b, real32_T y[12]) -{ - int32_T k; - for (k = 0; k < 12; k++) { - y[k] = rt_powf_snf(a[k], (real32_T)b); - } -} - -/* End of code generation (power.c) */ diff --git a/apps/attitude_estimator_ekf/codegen/power.h b/apps/attitude_estimator_ekf/codegen/power.h deleted file mode 100755 index a60f1bb25..000000000 --- a/apps/attitude_estimator_ekf/codegen/power.h +++ /dev/null @@ -1,34 +0,0 @@ -/* - * power.h - * - * Code generation for function 'power' - * - * C source code generated on: Mon Oct 01 19:38:49 2012 - * - */ - -#ifndef __POWER_H__ -#define __POWER_H__ -/* Include files */ -#include -#include -#include -#include -#include "rt_defines.h" -#include "rt_nonfinite.h" - -#include "rtwtypes.h" -#include "attitudeKalmanfilter_types.h" - -/* Type Definitions */ - -/* Named Constants */ - -/* Variable Declarations */ - -/* Variable Definitions */ - -/* Function Declarations */ -extern void power(const real32_T a[12], real_T b, real32_T y[12]); -#endif -/* End of code generation (power.h) */ diff --git a/apps/attitude_estimator_ekf/codegen/rdivide.c b/apps/attitude_estimator_ekf/codegen/rdivide.c new file mode 100755 index 000000000..32d75bf28 --- /dev/null +++ b/apps/attitude_estimator_ekf/codegen/rdivide.c @@ -0,0 +1,38 @@ +/* + * rdivide.c + * + * Code generation for function 'rdivide' + * + * C source code generated on: Sat Oct 13 16:28:18 2012 + * + */ + +/* Include files */ +#include "rt_nonfinite.h" +#include "attitudeKalmanfilter.h" +#include "rdivide.h" + +/* Type Definitions */ + +/* Named Constants */ + +/* Variable Declarations */ + +/* Variable Definitions */ + +/* Function Declarations */ + +/* Function Definitions */ + +/* + * + */ +void rdivide(const real32_T x[3], real32_T y, real32_T z[3]) +{ + int32_T i; + for (i = 0; i < 3; i++) { + z[i] = x[i] / y; + } +} + +/* End of code generation (rdivide.c) */ diff --git a/apps/attitude_estimator_ekf/codegen/rdivide.h b/apps/attitude_estimator_ekf/codegen/rdivide.h new file mode 100755 index 000000000..e488ed87f --- /dev/null +++ b/apps/attitude_estimator_ekf/codegen/rdivide.h @@ -0,0 +1,34 @@ +/* + * rdivide.h + * + * Code generation for function 'rdivide' + * + * C source code generated on: Sat Oct 13 16:28:18 2012 + * + */ + +#ifndef __RDIVIDE_H__ +#define __RDIVIDE_H__ +/* Include files */ +#include +#include +#include +#include +#include "rt_defines.h" +#include "rt_nonfinite.h" + +#include "rtwtypes.h" +#include "attitudeKalmanfilter_types.h" + +/* Type Definitions */ + +/* Named Constants */ + +/* Variable Declarations */ + +/* Variable Definitions */ + +/* Function Declarations */ +extern void rdivide(const real32_T x[3], real32_T y, real32_T z[3]); +#endif +/* End of code generation (rdivide.h) */ diff --git a/apps/attitude_estimator_ekf/codegen/rtGetInf.c b/apps/attitude_estimator_ekf/codegen/rtGetInf.c index 53686acc9..5500ef373 100755 --- a/apps/attitude_estimator_ekf/codegen/rtGetInf.c +++ b/apps/attitude_estimator_ekf/codegen/rtGetInf.c @@ -3,7 +3,7 @@ * * Code generation for function 'attitudeKalmanfilter' * - * C source code generated on: Mon Oct 01 19:38:49 2012 + * C source code generated on: Sat Oct 13 16:28:18 2012 * */ diff --git a/apps/attitude_estimator_ekf/codegen/rtGetInf.h b/apps/attitude_estimator_ekf/codegen/rtGetInf.h index 5ac1dc794..bfef3881e 100755 --- a/apps/attitude_estimator_ekf/codegen/rtGetInf.h +++ b/apps/attitude_estimator_ekf/codegen/rtGetInf.h @@ -3,7 +3,7 @@ * * Code generation for function 'attitudeKalmanfilter' * - * C source code generated on: Mon Oct 01 19:38:49 2012 + * C source code generated on: Sat Oct 13 16:28:18 2012 * */ diff --git a/apps/attitude_estimator_ekf/codegen/rtGetNaN.c b/apps/attitude_estimator_ekf/codegen/rtGetNaN.c index 1e1876b80..e5113aef0 100755 --- a/apps/attitude_estimator_ekf/codegen/rtGetNaN.c +++ b/apps/attitude_estimator_ekf/codegen/rtGetNaN.c @@ -3,7 +3,7 @@ * * Code generation for function 'attitudeKalmanfilter' * - * C source code generated on: Mon Oct 01 19:38:49 2012 + * C source code generated on: Sat Oct 13 16:28:18 2012 * */ diff --git a/apps/attitude_estimator_ekf/codegen/rtGetNaN.h b/apps/attitude_estimator_ekf/codegen/rtGetNaN.h index 5f1c81f67..28f35e1f1 100755 --- a/apps/attitude_estimator_ekf/codegen/rtGetNaN.h +++ b/apps/attitude_estimator_ekf/codegen/rtGetNaN.h @@ -3,7 +3,7 @@ * * Code generation for function 'attitudeKalmanfilter' * - * C source code generated on: Mon Oct 01 19:38:49 2012 + * C source code generated on: Sat Oct 13 16:28:18 2012 * */ diff --git a/apps/attitude_estimator_ekf/codegen/rt_defines.h b/apps/attitude_estimator_ekf/codegen/rt_defines.h index 5f65f6de9..3ce17dd0f 100755 --- a/apps/attitude_estimator_ekf/codegen/rt_defines.h +++ b/apps/attitude_estimator_ekf/codegen/rt_defines.h @@ -3,7 +3,7 @@ * * Code generation for function 'attitudeKalmanfilter' * - * C source code generated on: Mon Oct 01 19:38:49 2012 + * C source code generated on: Sat Oct 13 16:28:18 2012 * */ diff --git a/apps/attitude_estimator_ekf/codegen/rt_nonfinite.c b/apps/attitude_estimator_ekf/codegen/rt_nonfinite.c index 2c687d7a1..6f58e7265 100755 --- a/apps/attitude_estimator_ekf/codegen/rt_nonfinite.c +++ b/apps/attitude_estimator_ekf/codegen/rt_nonfinite.c @@ -3,7 +3,7 @@ * * Code generation for function 'attitudeKalmanfilter' * - * C source code generated on: Mon Oct 01 19:38:49 2012 + * C source code generated on: Sat Oct 13 16:28:18 2012 * */ diff --git a/apps/attitude_estimator_ekf/codegen/rt_nonfinite.h b/apps/attitude_estimator_ekf/codegen/rt_nonfinite.h index d2ebd0806..5c2a0aa61 100755 --- a/apps/attitude_estimator_ekf/codegen/rt_nonfinite.h +++ b/apps/attitude_estimator_ekf/codegen/rt_nonfinite.h @@ -3,7 +3,7 @@ * * Code generation for function 'attitudeKalmanfilter' * - * C source code generated on: Mon Oct 01 19:38:49 2012 + * C source code generated on: Sat Oct 13 16:28:18 2012 * */ diff --git a/apps/attitude_estimator_ekf/codegen/rtwtypes.h b/apps/attitude_estimator_ekf/codegen/rtwtypes.h index b487c8b38..4fbb93f57 100755 --- a/apps/attitude_estimator_ekf/codegen/rtwtypes.h +++ b/apps/attitude_estimator_ekf/codegen/rtwtypes.h @@ -3,7 +3,7 @@ * * Code generation for function 'attitudeKalmanfilter' * - * C source code generated on: Mon Oct 01 19:38:49 2012 + * C source code generated on: Sat Oct 13 16:28:18 2012 * */ @@ -26,8 +26,8 @@ * Number of bits: char: 8 short: 16 int: 32 * long: 32 native word size: 32 * Byte ordering: LittleEndian - * Signed integer division rounds to: Zero - * Shift right on a signed integer as arithmetic shift: on + * Signed integer division rounds to: Undefined + * Shift right on a signed integer as arithmetic shift: off *=======================================================================*/ /*=======================================================================* -- cgit v1.2.3