diff options
| author | Lorenz Meier <lm@inf.ethz.ch> | 2012-10-16 11:10:09 +0200 |
|---|---|---|
| committer | Lorenz Meier <lm@inf.ethz.ch> | 2012-10-16 11:10:09 +0200 |
| commit | fef4362e79fb05ddf8caba4bb4365bab13b39ce6 (patch) | |
| tree | 726dddc90d056bca5db13e4ea337c1e1e13787a7 /apps/attitude_estimator_ekf | |
| parent | a720bfff5e7562870a1f3a82bd2fc8da1d660658 (diff) | |
| download | px4-firmware-fef4362e79fb05ddf8caba4bb4365bab13b39ce6.tar.gz px4-firmware-fef4362e79fb05ddf8caba4bb4365bab13b39ce6.tar.bz2 px4-firmware-fef4362e79fb05ddf8caba4bb4365bab13b39ce6.zip | |
Merged new EKF version
Diffstat (limited to 'apps/attitude_estimator_ekf')
33 files changed, 670 insertions, 685 deletions
diff --git a/apps/attitude_estimator_ekf/.context b/apps/attitude_estimator_ekf/.context deleted file mode 100644 index e69de29bb..000000000 --- a/apps/attitude_estimator_ekf/.context +++ /dev/null diff --git a/apps/attitude_estimator_ekf/Makefile b/apps/attitude_estimator_ekf/Makefile index e5620138a..d6ec98f0b 100644..100755 --- 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 index 1d4df87fe..b507b4c10 100644..100755 --- 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 index d95c56368..f20d1b204 100644..100755 --- 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 index 6a63f9767..6a63f9767 100644..100755 --- a/apps/attitude_estimator_ekf/attitude_estimator_ekf_params.h +++ b/apps/attitude_estimator_ekf/attitude_estimator_ekf_params.h 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 <math.h>
-#include <stdio.h>
+#include <stddef.h>
#include <stdlib.h>
#include <string.h>
#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 <math.h>
-#include <stdio.h>
+#include <stddef.h>
#include <stdlib.h>
#include <string.h>
#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 <math.h>
-#include <stdio.h>
+#include <stddef.h>
#include <stdlib.h>
#include <string.h>
#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 <math.h>
-#include <stdio.h>
+#include <stddef.h>
#include <stdlib.h>
#include <string.h>
#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 <math.h>
-#include <stdio.h>
+#include <stddef.h>
#include <stdlib.h>
#include <string.h>
#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 <math.h>
-#include <stdio.h>
+#include <stddef.h>
#include <stdlib.h>
#include <string.h>
#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 <math.h>
-#include <stdio.h>
+#include <stddef.h>
#include <stdlib.h>
#include <string.h>
#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 <math.h>
-#include <stdio.h>
+#include <stddef.h>
#include <stdlib.h>
#include <string.h>
#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/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/power.h b/apps/attitude_estimator_ekf/codegen/rdivide.h index a60f1bb25..e488ed87f 100755 --- a/apps/attitude_estimator_ekf/codegen/power.h +++ b/apps/attitude_estimator_ekf/codegen/rdivide.h @@ -1,17 +1,17 @@ /*
- * power.h
+ * rdivide.h
*
- * Code generation for function 'power'
+ * Code generation for function 'rdivide'
*
- * C source code generated on: Mon Oct 01 19:38:49 2012
+ * C source code generated on: Sat Oct 13 16:28:18 2012
*
*/
-#ifndef __POWER_H__
-#define __POWER_H__
+#ifndef __RDIVIDE_H__
+#define __RDIVIDE_H__
/* Include files */
#include <math.h>
-#include <stdio.h>
+#include <stddef.h>
#include <stdlib.h>
#include <string.h>
#include "rt_defines.h"
@@ -29,6 +29,6 @@ /* Variable Definitions */
/* Function Declarations */
-extern void power(const real32_T a[12], real_T b, real32_T y[12]);
+extern void rdivide(const real32_T x[3], real32_T y, real32_T z[3]);
#endif
-/* End of code generation (power.h) */
+/* 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
*=======================================================================*/
/*=======================================================================*
|