diff options
Diffstat (limited to 'src/modules/attitude_estimator_ekf')
29 files changed, 3336 insertions, 0 deletions
diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp new file mode 100755 index 000000000..8e18c3c9a --- /dev/null +++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp @@ -0,0 +1,471 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Author: Tobias Naegeli <naegelit@student.ethz.ch> + * Lorenz Meier <lm@inf.ethz.ch> + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/* + * @file attitude_estimator_ekf_main.c + * + * Extended Kalman Filter for Attitude Estimation. + */ + +#include <nuttx/config.h> +#include <unistd.h> +#include <stdlib.h> +#include <stdio.h> +#include <stdbool.h> +#include <poll.h> +#include <fcntl.h> +#include <float.h> +#include <nuttx/sched.h> +#include <sys/prctl.h> +#include <termios.h> +#include <errno.h> +#include <limits.h> +#include <math.h> +#include <uORB/uORB.h> +#include <uORB/topics/debug_key_value.h> +#include <uORB/topics/sensor_combined.h> +#include <uORB/topics/vehicle_attitude.h> +#include <uORB/topics/vehicle_status.h> +#include <uORB/topics/parameter_update.h> +#include <drivers/drv_hrt.h> + +#include <systemlib/systemlib.h> +#include <systemlib/perf_counter.h> +#include <systemlib/err.h> + +#ifdef __cplusplus +extern "C" { +#endif +#include "codegen/attitudeKalmanfilter_initialize.h" +#include "codegen/attitudeKalmanfilter.h" +#include "attitude_estimator_ekf_params.h" +#ifdef __cplusplus +} +#endif + +extern "C" __EXPORT int attitude_estimator_ekf_main(int argc, char *argv[]); + +static bool thread_should_exit = false; /**< Deamon exit flag */ +static bool thread_running = false; /**< Deamon status flag */ +static int attitude_estimator_ekf_task; /**< Handle of deamon task / thread */ + +/** + * Mainloop of attitude_estimator_ekf. + */ +int attitude_estimator_ekf_thread_main(int argc, char *argv[]); + +/** + * Print the correct usage. + */ +static void usage(const char *reason); + +static void +usage(const char *reason) +{ + if (reason) + fprintf(stderr, "%s\n", reason); + + fprintf(stderr, "usage: attitude_estimator_ekf {start|stop|status} [-p <additional params>]\n\n"); + exit(1); +} + +/** + * The attitude_estimator_ekf app only briefly exists to start + * the background job. The stack size assigned in the + * Makefile does only apply to this management task. + * + * The actual stack size should be set in the call + * to task_create(). + */ +int attitude_estimator_ekf_main(int argc, char *argv[]) +{ + if (argc < 1) + usage("missing command"); + + if (!strcmp(argv[1], "start")) { + + if (thread_running) { + printf("attitude_estimator_ekf already running\n"); + /* this is not an error */ + exit(0); + } + + thread_should_exit = false; + attitude_estimator_ekf_task = task_spawn("attitude_estimator_ekf", + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 5, + 12400, + attitude_estimator_ekf_thread_main, + (argv) ? (const char **)&argv[2] : (const char **)NULL); + exit(0); + } + + if (!strcmp(argv[1], "stop")) { + thread_should_exit = true; + exit(0); + } + + if (!strcmp(argv[1], "status")) { + if (thread_running) { + printf("\tattitude_estimator_ekf app is running\n"); + + } else { + printf("\tattitude_estimator_ekf app not started\n"); + } + + exit(0); + } + + usage("unrecognized command"); + exit(1); +} + +/* + * [Rot_matrix,x_aposteriori,P_aposteriori] = attitudeKalmanfilter(dt,z_k,x_aposteriori_k,P_aposteriori_k,knownConst) + */ + +/* + * EKF Attitude Estimator main function. + * + * Estimates the attitude recursively once started. + * + * @param argc number of commandline arguments (plus command name) + * @param argv strings containing the arguments + */ +int attitude_estimator_ekf_thread_main(int argc, char *argv[]) +{ + +const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds + + float dt = 0.005f; +/* state vector x has the following entries [ax,ay,az||mx,my,mz||wox,woy,woz||wx,wy,wz]' */ + float z_k[9] = {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 9.81f, 0.2f, -0.2f, 0.2f}; /**< Measurement vector */ + float x_aposteriori_k[12]; /**< states */ + 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, + }; /**< init: diagonal matrix with big values */ + + float x_aposteriori[12]; + float P_aposteriori[144]; + + /* output euler angles */ + float euler[3] = {0.0f, 0.0f, 0.0f}; + + float Rot_matrix[9] = {1.f, 0, 0, + 0, 1.f, 0, + 0, 0, 1.f + }; /**< init: identity matrix */ + + // print text + printf("Extended Kalman Filter Attitude Estimator initialized..\n\n"); + fflush(stdout); + + int overloadcounter = 19; + + /* Initialize filter */ + attitudeKalmanfilter_initialize(); + + /* store start time to guard against too slow update rates */ + uint64_t last_run = hrt_absolute_time(); + + struct sensor_combined_s raw; + memset(&raw, 0, sizeof(raw)); + struct vehicle_attitude_s att; + memset(&att, 0, sizeof(att)); + struct vehicle_status_s state; + memset(&state, 0, sizeof(state)); + + uint64_t last_data = 0; + uint64_t last_measurement = 0; + + /* subscribe to raw data */ + int sub_raw = orb_subscribe(ORB_ID(sensor_combined)); + /* rate-limit raw data updates to 200Hz */ + orb_set_interval(sub_raw, 4); + + /* subscribe to param changes */ + int sub_params = orb_subscribe(ORB_ID(parameter_update)); + + /* subscribe to system state*/ + int sub_state = orb_subscribe(ORB_ID(vehicle_status)); + + /* advertise attitude */ + orb_advert_t pub_att = orb_advertise(ORB_ID(vehicle_attitude), &att); + + + int loopcounter = 0; + int printcounter = 0; + + thread_running = true; + + /* advertise debug value */ + // struct debug_key_value_s dbg = { .key = "", .value = 0.0f }; + // orb_advert_t pub_dbg = -1; + + float sensor_update_hz[3] = {0.0f, 0.0f, 0.0f}; + // XXX write this out to perf regs + + /* keep track of sensor updates */ + uint32_t sensor_last_count[3] = {0, 0, 0}; + uint64_t sensor_last_timestamp[3] = {0, 0, 0}; + + struct attitude_estimator_ekf_params ekf_params; + + struct attitude_estimator_ekf_param_handles ekf_param_handles; + + /* 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; + + /* register the perf counter */ + perf_counter_t ekf_loop_perf = perf_alloc(PC_ELAPSED, "attitude_estimator_ekf"); + + /* Main loop*/ + while (!thread_should_exit) { + + struct pollfd fds[2]; + fds[0].fd = sub_raw; + fds[0].events = POLLIN; + fds[1].fd = sub_params; + fds[1].events = POLLIN; + int ret = poll(fds, 2, 1000); + + if (ret < 0) { + /* XXX this is seriously bad - should be an emergency */ + } else if (ret == 0) { + /* check if we're in HIL - not getting sensor data is fine then */ + orb_copy(ORB_ID(vehicle_status), sub_state, &state); + + if (!state.flag_hil_enabled) { + fprintf(stderr, + "[att ekf] WARNING: Not getting sensors - sensor app running?\n"); + } + + } else { + + /* only update parameters if they changed */ + if (fds[1].revents & POLLIN) { + /* read from param to clear updated flag */ + struct parameter_update_s update; + orb_copy(ORB_ID(parameter_update), sub_params, &update); + + /* update parameters */ + parameters_update(&ekf_param_handles, &ekf_params); + } + + /* only run filter if sensor values changed */ + if (fds[0].revents & POLLIN) { + + /* get latest measurements */ + orb_copy(ORB_ID(sensor_combined), sub_raw, &raw); + + 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]; + offset_count++; + + 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 { + + perf_begin(ekf_loop_perf); + + /* 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] - gyro_offsets[0]; + z_k[1] = raw.gyro_rad_s[1] - gyro_offsets[1]; + z_k[2] = raw.gyro_rad_s[2] - gyro_offsets[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++; + } + + static bool const_initialized = false; + + /* initialize with good values once we have a reasonable dt estimate */ + if (!const_initialized && dt < 0.05f && dt > 0.005f) { + 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; + } + + uint64_t timing_start = hrt_absolute_time(); + + 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, check for fatal inputs */ + if (isfinite(euler[0]) && isfinite(euler[1]) && isfinite(euler[2])) { + memcpy(P_aposteriori_k, P_aposteriori, sizeof(P_aposteriori_k)); + memcpy(x_aposteriori_k, x_aposteriori, sizeof(x_aposteriori_k)); + + } else { + /* due to inputs or numerical failure the output is invalid, skip it */ + continue; + } + + 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; + + // XXX Apply the same transformation to the rotation matrix + att.roll = euler[0] - ekf_params.roll_off; + att.pitch = euler[1] - ekf_params.pitch_off; + att.yaw = euler[2] - ekf_params.yaw_off; + + att.rollspeed = x_aposteriori[0]; + att.pitchspeed = x_aposteriori[1]; + att.yawspeed = x_aposteriori[2]; + att.rollacc = x_aposteriori[3]; + att.pitchacc = x_aposteriori[4]; + att.yawacc = x_aposteriori[5]; + + //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; + + if (isfinite(att.roll) && isfinite(att.pitch) && isfinite(att.yaw)) { + // Broadcast + orb_publish(ORB_ID(vehicle_attitude), pub_att, &att); + + } else { + warnx("NaN in roll/pitch/yaw estimate!"); + } + + perf_end(ekf_loop_perf); + } + } + } + + loopcounter++; + } + + thread_running = false; + + return 0; +} diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c new file mode 100755 index 000000000..7d3812abd --- /dev/null +++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c @@ -0,0 +1,105 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Author: Tobias Naegeli <naegelit@student.ethz.ch> + * Lorenz Meier <lm@inf.ethz.ch> + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/* + * @file attitude_estimator_ekf_params.c + * + * Parameters for EKF filter + */ + +#include "attitude_estimator_ekf_params.h" + +/* Extended Kalman Filter covariances */ + +/* gyro process noise */ +PARAM_DEFINE_FLOAT(EKF_ATT_V2_Q0, 1e-4f); +PARAM_DEFINE_FLOAT(EKF_ATT_V2_Q1, 0.08f); +PARAM_DEFINE_FLOAT(EKF_ATT_V2_Q2, 0.009f); +/* gyro offsets process noise */ +PARAM_DEFINE_FLOAT(EKF_ATT_V2_Q3, 0.005f); +PARAM_DEFINE_FLOAT(EKF_ATT_V2_Q4, 0.0f); + +/* gyro measurement noise */ +PARAM_DEFINE_FLOAT(EKF_ATT_V2_R0, 0.0008f); +PARAM_DEFINE_FLOAT(EKF_ATT_V2_R1, 0.8f); +PARAM_DEFINE_FLOAT(EKF_ATT_V2_R2, 1.0f); +/* accelerometer measurement noise */ +PARAM_DEFINE_FLOAT(EKF_ATT_V2_R3, 0.0f); + +/* offsets in roll, pitch and yaw of sensor plane and body */ +PARAM_DEFINE_FLOAT(ATT_ROLL_OFFS, 0.0f); +PARAM_DEFINE_FLOAT(ATT_PITCH_OFFS, 0.0f); +PARAM_DEFINE_FLOAT(ATT_YAW_OFFS, 0.0f); + +int parameters_init(struct attitude_estimator_ekf_param_handles *h) +{ + /* PID parameters */ + h->q0 = param_find("EKF_ATT_V2_Q0"); + h->q1 = param_find("EKF_ATT_V2_Q1"); + h->q2 = param_find("EKF_ATT_V2_Q2"); + h->q3 = param_find("EKF_ATT_V2_Q3"); + h->q4 = param_find("EKF_ATT_V2_Q4"); + + h->r0 = param_find("EKF_ATT_V2_R0"); + h->r1 = param_find("EKF_ATT_V2_R1"); + h->r2 = param_find("EKF_ATT_V2_R2"); + h->r3 = param_find("EKF_ATT_V2_R3"); + + h->roll_off = param_find("ATT_ROLL_OFFS"); + h->pitch_off = param_find("ATT_PITCH_OFFS"); + h->yaw_off = param_find("ATT_YAW_OFFS"); + + return OK; +} + +int parameters_update(const struct attitude_estimator_ekf_param_handles *h, struct attitude_estimator_ekf_params *p) +{ + param_get(h->q0, &(p->q[0])); + param_get(h->q1, &(p->q[1])); + param_get(h->q2, &(p->q[2])); + param_get(h->q3, &(p->q[3])); + param_get(h->q4, &(p->q[4])); + + param_get(h->r0, &(p->r[0])); + param_get(h->r1, &(p->r[1])); + param_get(h->r2, &(p->r[2])); + param_get(h->r3, &(p->r[3])); + + param_get(h->roll_off, &(p->roll_off)); + param_get(h->pitch_off, &(p->pitch_off)); + param_get(h->yaw_off, &(p->yaw_off)); + + return OK; +} diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.h b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.h new file mode 100755 index 000000000..09817d58e --- /dev/null +++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.h @@ -0,0 +1,68 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Author: Tobias Naegeli <naegelit@student.ethz.ch> + * Lorenz Meier <lm@inf.ethz.ch> + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/* + * @file attitude_estimator_ekf_params.h + * + * Parameters for EKF filter + */ + +#include <systemlib/param/param.h> + +struct attitude_estimator_ekf_params { + float r[9]; + float q[12]; + float roll_off; + float pitch_off; + float yaw_off; +}; + +struct attitude_estimator_ekf_param_handles { + param_t r0, r1, r2, r3; + param_t q0, q1, q2, q3, q4; + param_t roll_off, pitch_off, yaw_off; +}; + +/** + * Initialize all parameter handles and values + * + */ +int parameters_init(struct attitude_estimator_ekf_param_handles *h); + +/** + * Update all parameters + * + */ +int parameters_update(const struct attitude_estimator_ekf_param_handles *h, struct attitude_estimator_ekf_params *p); diff --git a/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter.c b/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter.c new file mode 100755 index 000000000..9e97ad11a --- /dev/null +++ b/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter.c @@ -0,0 +1,1148 @@ +/*
+ * attitudeKalmanfilter.c
+ *
+ * Code generation for function 'attitudeKalmanfilter'
+ *
+ * C source code generated on: Sat Jan 19 15:25:29 2013
+ *
+ */
+
+/* Include files */
+#include "rt_nonfinite.h"
+#include "attitudeKalmanfilter.h"
+#include "rdivide.h"
+#include "norm.h"
+#include "cross.h"
+#include "eye.h"
+#include "mrdivide.h"
+
+/* Type Definitions */
+
+/* Named Constants */
+
+/* Variable Declarations */
+
+/* Variable Definitions */
+
+/* Function Declarations */
+static real32_T rt_atan2f_snf(real32_T u0, real32_T u1);
+
+/* Function Definitions */
+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)) {
+ 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;
+ } else if (u0 < 0.0F) {
+ y = -(RT_PIF / 2.0F);
+ } else {
+ y = 0.0F;
+ }
+ } else {
+ y = (real32_T)atan2(u0, u1);
+ }
+
+ return y;
+}
+
+/*
+ * function [eulerAngles,Rot_matrix,x_aposteriori,P_aposteriori] = attitudeKalmanfilter_wo(updateVect,dt,z,x_aposteriori_k,P_aposteriori_k,q,r)
+ */
+void attitudeKalmanfilter(const uint8_T updateVect[3], real32_T dt, const
+ real32_T z[9], const real32_T x_aposteriori_k[12], const real32_T
+ P_aposteriori_k[144], const real32_T q[12], real32_T r[9], real32_T
+ eulerAngles[3], real32_T Rot_matrix[9], real32_T x_aposteriori[12], real32_T
+ P_aposteriori[144])
+{
+ real32_T wak[3];
+ real32_T O[9];
+ real_T dv0[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 z_n_b[3];
+ real32_T c_a[3];
+ real32_T d_a[3];
+ int32_T i0;
+ real32_T x_apriori[12];
+ real_T dv1[144];
+ real32_T A_lin[144];
+ static const int8_T iv0[36] = { 0, 0, 0, 0, 0, 0, 0, 0, 0, 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 };
+
+ real32_T b_A_lin[144];
+ real32_T b_q[144];
+ real32_T c_A_lin[144];
+ real32_T d_A_lin[144];
+ real32_T e_A_lin[144];
+ int32_T i1;
+ real32_T P_apriori[144];
+ real32_T b_P_apriori[108];
+ static const int8_T iv1[108] = { 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, 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 };
+
+ real32_T K_k[108];
+ real32_T fv0[81];
+ static const int8_T iv2[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, 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, 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 b_r[81];
+ real32_T fv1[81];
+ real32_T f0;
+ real32_T c_P_apriori[36];
+ static const int8_T iv3[36] = { 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 };
+
+ real32_T fv2[36];
+ static const int8_T iv4[36] = { 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 };
+
+ real32_T c_r[9];
+ real32_T b_K_k[36];
+ real32_T d_P_apriori[72];
+ static const int8_T iv5[72] = { 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, 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 c_K_k[72];
+ static const int8_T iv6[72] = { 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, 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_z[6];
+ static const int8_T iv7[72] = { 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, 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 };
+
+ static const int8_T iv8[72] = { 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, 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 };
+
+ real32_T fv3[6];
+ real32_T c_z[6];
+
+ /* Extended Attitude Kalmanfilter */
+ /* */
+ /* state vector x has the following entries [ax,ay,az||mx,my,mz||wox,woy,woz||wx,wy,wz]' */
+ /* measurement vector z has the following entries [ax,ay,az||mx,my,mz||wmx,wmy,wmz]' */
+ /* knownConst has the following entries [PrvaA,PrvarM,PrvarWO,PrvarW||MsvarA,MsvarM,MsvarW] */
+ /* */
+ /* [x_aposteriori,P_aposteriori] = AttKalman(dt,z_k,x_aposteriori_k,P_aposteriori_k,knownConst) */
+ /* */
+ /* Example.... */
+ /* */
+ /* $Author: Tobias Naegeli $ $Date: 2012 $ $Revision: 1 $ */
+ /* coder.varsize('udpIndVect', [9,1], [1,0]) */
+ /* udpIndVect=find(updVect); */
+ /* process and measurement noise covariance matrix */
+ /* Q = diag(q.^2*dt); */
+ /* observation matrix */
+ /* 'attitudeKalmanfilter:33' wx= x_aposteriori_k(1); */
+ /* 'attitudeKalmanfilter:34' wy= x_aposteriori_k(2); */
+ /* 'attitudeKalmanfilter:35' wz= x_aposteriori_k(3); */
+ /* 'attitudeKalmanfilter:37' wax= x_aposteriori_k(4); */
+ /* 'attitudeKalmanfilter:38' way= x_aposteriori_k(5); */
+ /* 'attitudeKalmanfilter:39' waz= x_aposteriori_k(6); */
+ /* 'attitudeKalmanfilter:41' zex= x_aposteriori_k(7); */
+ /* 'attitudeKalmanfilter:42' zey= x_aposteriori_k(8); */
+ /* 'attitudeKalmanfilter:43' zez= x_aposteriori_k(9); */
+ /* 'attitudeKalmanfilter:45' mux= x_aposteriori_k(10); */
+ /* 'attitudeKalmanfilter:46' muy= x_aposteriori_k(11); */
+ /* 'attitudeKalmanfilter:47' muz= x_aposteriori_k(12); */
+ /* % prediction section */
+ /* body angular accelerations */
+ /* 'attitudeKalmanfilter:51' wak =[wax;way;waz]; */
+ wak[0] = x_aposteriori_k[3];
+ wak[1] = x_aposteriori_k[4];
+ wak[2] = x_aposteriori_k[5];
+
+ /* body angular rates */
+ /* 'attitudeKalmanfilter:54' wk =[wx; wy; wz] + dt*wak; */
+ /* derivative of the prediction rotation matrix */
+ /* 'attitudeKalmanfilter:57' 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];
+ O[3] = x_aposteriori_k[2];
+ O[4] = 0.0F;
+ O[5] = -x_aposteriori_k[0];
+ O[6] = -x_aposteriori_k[1];
+ O[7] = x_aposteriori_k[0];
+ O[8] = 0.0F;
+
+ /* prediction of the earth z vector */
+ /* 'attitudeKalmanfilter:60' zek =(eye(3)+O*dt)*[zex;zey;zez]; */
+ eye(dv0);
+ for (i = 0; i < 9; i++) {
+ a[i] = (real32_T)dv0[i] + O[i] * dt;
+ }
+
+ /* prediction of the magnetic vector */
+ /* 'attitudeKalmanfilter:63' muk =(eye(3)+O*dt)*[mux;muy;muz]; */
+ eye(dv0);
+ for (i = 0; i < 9; i++) {
+ b_a[i] = (real32_T)dv0[i] + O[i] * dt;
+ }
+
+ /* 'attitudeKalmanfilter:65' EZ=[0,zez,-zey; */
+ /* 'attitudeKalmanfilter:66' -zez,0,zex; */
+ /* 'attitudeKalmanfilter:67' zey,-zex,0]'; */
+ /* 'attitudeKalmanfilter:68' MA=[0,muz,-muy; */
+ /* 'attitudeKalmanfilter:69' -muz,0,mux; */
+ /* 'attitudeKalmanfilter:70' zey,-mux,0]'; */
+ /* 'attitudeKalmanfilter:74' E=eye(3); */
+ /* 'attitudeKalmanfilter:76' Z=zeros(3); */
+ /* 'attitudeKalmanfilter:77' x_apriori=[wk;wak;zek;muk]; */
+ x_n_b[0] = x_aposteriori_k[0];
+ x_n_b[1] = x_aposteriori_k[1];
+ x_n_b[2] = x_aposteriori_k[2];
+ b_x_aposteriori_k[0] = x_aposteriori_k[6];
+ b_x_aposteriori_k[1] = x_aposteriori_k[7];
+ b_x_aposteriori_k[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];
+ for (i = 0; i < 3; i++) {
+ c_a[i] = 0.0F;
+ for (i0 = 0; i0 < 3; i0++) {
+ c_a[i] += a[i + 3 * i0] * b_x_aposteriori_k[i0];
+ }
+
+ d_a[i] = 0.0F;
+ for (i0 = 0; i0 < 3; i0++) {
+ d_a[i] += b_a[i + 3 * i0] * z_n_b[i0];
+ }
+
+ x_apriori[i] = x_n_b[i] + dt * wak[i];
+ }
+
+ for (i = 0; i < 3; i++) {
+ x_apriori[i + 3] = wak[i];
+ }
+
+ for (i = 0; i < 3; i++) {
+ x_apriori[i + 6] = c_a[i];
+ }
+
+ for (i = 0; i < 3; i++) {
+ x_apriori[i + 9] = d_a[i];
+ }
+
+ /* 'attitudeKalmanfilter:81' A_lin=[ Z, E, Z, Z */
+ /* 'attitudeKalmanfilter:82' Z, Z, Z, Z */
+ /* 'attitudeKalmanfilter:83' EZ, Z, O, Z */
+ /* 'attitudeKalmanfilter:84' MA, Z, Z, O]; */
+ /* 'attitudeKalmanfilter:86' A_lin=eye(12)+A_lin*dt; */
+ b_eye(dv1);
+ for (i = 0; i < 12; i++) {
+ for (i0 = 0; i0 < 3; i0++) {
+ A_lin[i0 + 12 * i] = (real32_T)iv0[i0 + 3 * i];
+ }
+
+ for (i0 = 0; i0 < 3; i0++) {
+ A_lin[(i0 + 12 * i) + 3] = 0.0F;
+ }
+ }
+
+ A_lin[6] = 0.0F;
+ A_lin[7] = x_aposteriori_k[8];
+ A_lin[8] = -x_aposteriori_k[7];
+ A_lin[18] = -x_aposteriori_k[8];
+ A_lin[19] = 0.0F;
+ A_lin[20] = x_aposteriori_k[6];
+ A_lin[30] = x_aposteriori_k[7];
+ A_lin[31] = -x_aposteriori_k[6];
+ A_lin[32] = 0.0F;
+ for (i = 0; i < 3; i++) {
+ for (i0 = 0; i0 < 3; i0++) {
+ A_lin[(i0 + 12 * (i + 3)) + 6] = 0.0F;
+ }
+ }
+
+ for (i = 0; i < 3; i++) {
+ for (i0 = 0; i0 < 3; i0++) {
+ A_lin[(i0 + 12 * (i + 6)) + 6] = O[i0 + 3 * i];
+ }
+ }
+
+ for (i = 0; i < 3; i++) {
+ for (i0 = 0; i0 < 3; i0++) {
+ A_lin[(i0 + 12 * (i + 9)) + 6] = 0.0F;
+ }
+ }
+
+ A_lin[9] = 0.0F;
+ A_lin[10] = x_aposteriori_k[11];
+ A_lin[11] = -x_aposteriori_k[10];
+ A_lin[21] = -x_aposteriori_k[11];
+ A_lin[22] = 0.0F;
+ A_lin[23] = x_aposteriori_k[9];
+ A_lin[33] = x_aposteriori_k[7];
+ A_lin[34] = -x_aposteriori_k[9];
+ A_lin[35] = 0.0F;
+ for (i = 0; i < 3; i++) {
+ for (i0 = 0; i0 < 3; i0++) {
+ A_lin[(i0 + 12 * (i + 3)) + 9] = 0.0F;
+ }
+ }
+
+ for (i = 0; i < 3; i++) {
+ for (i0 = 0; i0 < 3; i0++) {
+ A_lin[(i0 + 12 * (i + 6)) + 9] = 0.0F;
+ }
+ }
+
+ for (i = 0; i < 3; i++) {
+ for (i0 = 0; i0 < 3; i0++) {
+ A_lin[(i0 + 12 * (i + 9)) + 9] = O[i0 + 3 * i];
+ }
+ }
+
+ for (i = 0; i < 12; i++) {
+ for (i0 = 0; i0 < 12; i0++) {
+ b_A_lin[i0 + 12 * i] = (real32_T)dv1[i0 + 12 * i] + A_lin[i0 + 12 * i] *
+ dt;
+ }
+ }
+
+ /* 'attitudeKalmanfilter:88' Qtemp=[ q(1), 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0; */
+ /* 'attitudeKalmanfilter:89' 0, q(1), 0, 0, 0, 0, 0, 0, 0, 0, 0, 0; */
+ /* 'attitudeKalmanfilter:90' 0, 0, q(1), 0, 0, 0, 0, 0, 0, 0, 0, 0; */
+ /* 'attitudeKalmanfilter:91' 0, 0, 0, q(2), 0, 0, 0, 0, 0, 0, 0, 0; */
+ /* 'attitudeKalmanfilter:92' 0, 0, 0, 0, q(2), 0, 0, 0, 0, 0, 0, 0; */
+ /* 'attitudeKalmanfilter:93' 0, 0, 0, 0, 0, q(2), 0, 0, 0, 0, 0, 0; */
+ /* 'attitudeKalmanfilter:94' 0, 0, 0, 0, 0, 0, q(3), 0, 0, 0, 0, 0; */
+ /* 'attitudeKalmanfilter:95' 0, 0, 0, 0, 0, 0, 0, q(3), 0, 0, 0, 0; */
+ /* 'attitudeKalmanfilter:96' 0, 0, 0, 0, 0, 0, 0, 0, q(3), 0, 0, 0; */
+ /* 'attitudeKalmanfilter:97' 0, 0, 0, 0, 0, 0, 0, 0, 0, q(4), 0, 0; */
+ /* 'attitudeKalmanfilter:98' 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, q(4), 0; */
+ /* 'attitudeKalmanfilter:99' 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, q(4)]; */
+ /* 'attitudeKalmanfilter:103' Q=A_lin*Qtemp*A_lin'; */
+ /* 'attitudeKalmanfilter:106' P_apriori=A_lin*P_aposteriori_k*A_lin'+Q; */
+ b_q[0] = q[0];
+ b_q[12] = 0.0F;
+ b_q[24] = 0.0F;
+ b_q[36] = 0.0F;
+ b_q[48] = 0.0F;
+ b_q[60] = 0.0F;
+ b_q[72] = 0.0F;
+ b_q[84] = 0.0F;
+ b_q[96] = 0.0F;
+ b_q[108] = 0.0F;
+ b_q[120] = 0.0F;
+ b_q[132] = 0.0F;
+ b_q[1] = 0.0F;
+ b_q[13] = q[0];
+ b_q[25] = 0.0F;
+ b_q[37] = 0.0F;
+ b_q[49] = 0.0F;
+ b_q[61] = 0.0F;
+ b_q[73] = 0.0F;
+ b_q[85] = 0.0F;
+ b_q[97] = 0.0F;
+ b_q[109] = 0.0F;
+ b_q[121] = 0.0F;
+ b_q[133] = 0.0F;
+ b_q[2] = 0.0F;
+ b_q[14] = 0.0F;
+ b_q[26] = q[0];
+ b_q[38] = 0.0F;
+ b_q[50] = 0.0F;
+ b_q[62] = 0.0F;
+ b_q[74] = 0.0F;
+ b_q[86] = 0.0F;
+ b_q[98] = 0.0F;
+ b_q[110] = 0.0F;
+ b_q[122] = 0.0F;
+ b_q[134] = 0.0F;
+ b_q[3] = 0.0F;
+ b_q[15] = 0.0F;
+ b_q[27] = 0.0F;
+ b_q[39] = q[1];
+ b_q[51] = 0.0F;
+ b_q[63] = 0.0F;
+ b_q[75] = 0.0F;
+ b_q[87] = 0.0F;
+ b_q[99] = 0.0F;
+ b_q[111] = 0.0F;
+ b_q[123] = 0.0F;
+ b_q[135] = 0.0F;
+ b_q[4] = 0.0F;
+ b_q[16] = 0.0F;
+ b_q[28] = 0.0F;
+ b_q[40] = 0.0F;
+ b_q[52] = q[1];
+ b_q[64] = 0.0F;
+ b_q[76] = 0.0F;
+ b_q[88] = 0.0F;
+ b_q[100] = 0.0F;
+ b_q[112] = 0.0F;
+ b_q[124] = 0.0F;
+ b_q[136] = 0.0F;
+ b_q[5] = 0.0F;
+ b_q[17] = 0.0F;
+ b_q[29] = 0.0F;
+ b_q[41] = 0.0F;
+ b_q[53] = 0.0F;
+ b_q[65] = q[1];
+ b_q[77] = 0.0F;
+ b_q[89] = 0.0F;
+ b_q[101] = 0.0F;
+ b_q[113] = 0.0F;
+ b_q[125] = 0.0F;
+ b_q[137] = 0.0F;
+ b_q[6] = 0.0F;
+ b_q[18] = 0.0F;
+ b_q[30] = 0.0F;
+ b_q[42] = 0.0F;
+ b_q[54] = 0.0F;
+ b_q[66] = 0.0F;
+ b_q[78] = q[2];
+ b_q[90] = 0.0F;
+ b_q[102] = 0.0F;
+ b_q[114] = 0.0F;
+ b_q[126] = 0.0F;
+ b_q[138] = 0.0F;
+ b_q[7] = 0.0F;
+ b_q[19] = 0.0F;
+ b_q[31] = 0.0F;
+ b_q[43] = 0.0F;
+ b_q[55] = 0.0F;
+ b_q[67] = 0.0F;
+ b_q[79] = 0.0F;
+ b_q[91] = q[2];
+ b_q[103] = 0.0F;
+ b_q[115] = 0.0F;
+ b_q[127] = 0.0F;
+ b_q[139] = 0.0F;
+ b_q[8] = 0.0F;
+ b_q[20] = 0.0F;
+ b_q[32] = 0.0F;
+ b_q[44] = 0.0F;
+ b_q[56] = 0.0F;
+ b_q[68] = 0.0F;
+ b_q[80] = 0.0F;
+ b_q[92] = 0.0F;
+ b_q[104] = q[2];
+ b_q[116] = 0.0F;
+ b_q[128] = 0.0F;
+ b_q[140] = 0.0F;
+ b_q[9] = 0.0F;
+ b_q[21] = 0.0F;
+ b_q[33] = 0.0F;
+ b_q[45] = 0.0F;
+ b_q[57] = 0.0F;
+ b_q[69] = 0.0F;
+ b_q[81] = 0.0F;
+ b_q[93] = 0.0F;
+ b_q[105] = 0.0F;
+ b_q[117] = q[3];
+ b_q[129] = 0.0F;
+ b_q[141] = 0.0F;
+ b_q[10] = 0.0F;
+ b_q[22] = 0.0F;
+ b_q[34] = 0.0F;
+ b_q[46] = 0.0F;
+ b_q[58] = 0.0F;
+ b_q[70] = 0.0F;
+ b_q[82] = 0.0F;
+ b_q[94] = 0.0F;
+ b_q[106] = 0.0F;
+ b_q[118] = 0.0F;
+ b_q[130] = q[3];
+ b_q[142] = 0.0F;
+ b_q[11] = 0.0F;
+ b_q[23] = 0.0F;
+ b_q[35] = 0.0F;
+ b_q[47] = 0.0F;
+ b_q[59] = 0.0F;
+ b_q[71] = 0.0F;
+ b_q[83] = 0.0F;
+ b_q[95] = 0.0F;
+ b_q[107] = 0.0F;
+ b_q[119] = 0.0F;
+ b_q[131] = 0.0F;
+ b_q[143] = q[3];
+ for (i = 0; i < 12; i++) {
+ for (i0 = 0; i0 < 12; i0++) {
+ A_lin[i + 12 * i0] = 0.0F;
+ for (i1 = 0; i1 < 12; i1++) {
+ A_lin[i + 12 * i0] += b_A_lin[i + 12 * i1] * P_aposteriori_k[i1 + 12 *
+ i0];
+ }
+
+ c_A_lin[i + 12 * i0] = 0.0F;
+ for (i1 = 0; i1 < 12; i1++) {
+ c_A_lin[i + 12 * i0] += b_A_lin[i + 12 * i1] * b_q[i1 + 12 * i0];
+ }
+ }
+
+ for (i0 = 0; i0 < 12; i0++) {
+ d_A_lin[i + 12 * i0] = 0.0F;
+ for (i1 = 0; i1 < 12; i1++) {
+ d_A_lin[i + 12 * i0] += A_lin[i + 12 * i1] * b_A_lin[i0 + 12 * i1];
+ }
+
+ e_A_lin[i + 12 * i0] = 0.0F;
+ for (i1 = 0; i1 < 12; i1++) {
+ e_A_lin[i + 12 * i0] += c_A_lin[i + 12 * i1] * b_A_lin[i0 + 12 * i1];
+ }
+ }
+ }
+
+ for (i = 0; i < 12; i++) {
+ for (i0 = 0; i0 < 12; i0++) {
+ P_apriori[i0 + 12 * i] = d_A_lin[i0 + 12 * i] + e_A_lin[i0 + 12 * i];
+ }
+ }
+
+ /* % update */
+ /* 'attitudeKalmanfilter:110' if updateVect(1)==1&&updateVect(2)==1&&updateVect(3)==1 */
+ if ((updateVect[0] == 1) && (updateVect[1] == 1) && (updateVect[2] == 1)) {
+ /* 'attitudeKalmanfilter:111' if z(6)<4 || z(5)>15 */
+ if ((z[5] < 4.0F) || (z[4] > 15.0F)) {
+ /* 'attitudeKalmanfilter:112' r(2)=10000; */
+ r[1] = 10000.0F;
+ }
+
+ /* 'attitudeKalmanfilter:114' R=[r(1),0,0,0,0,0,0,0,0; */
+ /* 'attitudeKalmanfilter:115' 0,r(1),0,0,0,0,0,0,0; */
+ /* 'attitudeKalmanfilter:116' 0,0,r(1),0,0,0,0,0,0; */
+ /* 'attitudeKalmanfilter:117' 0,0,0,r(2),0,0,0,0,0; */
+ /* 'attitudeKalmanfilter:118' 0,0,0,0,r(2),0,0,0,0; */
+ /* 'attitudeKalmanfilter:119' 0,0,0,0,0,r(2),0,0,0; */
+ /* 'attitudeKalmanfilter:120' 0,0,0,0,0,0,r(3),0,0; */
+ /* 'attitudeKalmanfilter:121' 0,0,0,0,0,0,0,r(3),0; */
+ /* 'attitudeKalmanfilter:122' 0,0,0,0,0,0,0,0,r(3)]; */
+ /* observation matrix */
+ /* [zw;ze;zmk]; */
+ /* 'attitudeKalmanfilter:125' H_k=[ E, Z, Z, Z; */
+ /* 'attitudeKalmanfilter:126' Z, Z, E, Z; */
+ /* 'attitudeKalmanfilter:127' Z, Z, Z, E]; */
+ /* 'attitudeKalmanfilter:129' y_k=z(1:9)-H_k*x_apriori; */
+ /* 'attitudeKalmanfilter:132' S_k=H_k*P_apriori*H_k'+R; */
+ /* 'attitudeKalmanfilter:133' 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;
+ for (i1 = 0; i1 < 12; i1++) {
+ b_P_apriori[i + 12 * i0] += P_apriori[i + 12 * i1] * (real32_T)iv1[i1
+ + 12 * i0];
+ }
+ }
+ }
+
+ for (i = 0; i < 9; i++) {
+ for (i0 = 0; i0 < 12; i0++) {
+ K_k[i + 9 * i0] = 0.0F;
+ for (i1 = 0; i1 < 12; i1++) {
+ K_k[i + 9 * i0] += (real32_T)iv2[i + 9 * i1] * P_apriori[i1 + 12 * i0];
+ }
+ }
+
+ for (i0 = 0; i0 < 9; i0++) {
+ fv0[i + 9 * i0] = 0.0F;
+ for (i1 = 0; i1 < 12; i1++) {
+ fv0[i + 9 * i0] += K_k[i + 9 * i1] * (real32_T)iv1[i1 + 12 * i0];
+ }
+ }
+ }
+
+ b_r[0] = r[0];
+ b_r[9] = 0.0F;
+ b_r[18] = 0.0F;
+ b_r[27] = 0.0F;
+ b_r[36] = 0.0F;
+ b_r[45] = 0.0F;
+ b_r[54] = 0.0F;
+ b_r[63] = 0.0F;
+ b_r[72] = 0.0F;
+ b_r[1] = 0.0F;
+ b_r[10] = r[0];
+ b_r[19] = 0.0F;
+ b_r[28] = 0.0F;
+ b_r[37] = 0.0F;
+ b_r[46] = 0.0F;
+ b_r[55] = 0.0F;
+ b_r[64] = 0.0F;
+ b_r[73] = 0.0F;
+ b_r[2] = 0.0F;
+ b_r[11] = 0.0F;
+ b_r[20] = r[0];
+ b_r[29] = 0.0F;
+ b_r[38] = 0.0F;
+ b_r[47] = 0.0F;
+ b_r[56] = 0.0F;
+ b_r[65] = 0.0F;
+ b_r[74] = 0.0F;
+ b_r[3] = 0.0F;
+ b_r[12] = 0.0F;
+ b_r[21] = 0.0F;
+ b_r[30] = r[1];
+ b_r[39] = 0.0F;
+ b_r[48] = 0.0F;
+ b_r[57] = 0.0F;
+ b_r[66] = 0.0F;
+ b_r[75] = 0.0F;
+ b_r[4] = 0.0F;
+ b_r[13] = 0.0F;
+ b_r[22] = 0.0F;
+ b_r[31] = 0.0F;
+ b_r[40] = r[1];
+ b_r[49] = 0.0F;
+ b_r[58] = 0.0F;
+ b_r[67] = 0.0F;
+ b_r[76] = 0.0F;
+ b_r[5] = 0.0F;
+ b_r[14] = 0.0F;
+ b_r[23] = 0.0F;
+ b_r[32] = 0.0F;
+ b_r[41] = 0.0F;
+ b_r[50] = r[1];
+ b_r[59] = 0.0F;
+ b_r[68] = 0.0F;
+ b_r[77] = 0.0F;
+ b_r[6] = 0.0F;
+ b_r[15] = 0.0F;
+ b_r[24] = 0.0F;
+ b_r[33] = 0.0F;
+ b_r[42] = 0.0F;
+ b_r[51] = 0.0F;
+ b_r[60] = r[2];
+ b_r[69] = 0.0F;
+ b_r[78] = 0.0F;
+ b_r[7] = 0.0F;
+ b_r[16] = 0.0F;
+ b_r[25] = 0.0F;
+ b_r[34] = 0.0F;
+ b_r[43] = 0.0F;
+ b_r[52] = 0.0F;
+ b_r[61] = 0.0F;
+ b_r[70] = r[2];
+ b_r[79] = 0.0F;
+ b_r[8] = 0.0F;
+ b_r[17] = 0.0F;
+ b_r[26] = 0.0F;
+ b_r[35] = 0.0F;
+ b_r[44] = 0.0F;
+ b_r[53] = 0.0F;
+ b_r[62] = 0.0F;
+ b_r[71] = 0.0F;
+ b_r[80] = r[2];
+ for (i = 0; i < 9; i++) {
+ for (i0 = 0; i0 < 9; i0++) {
+ fv1[i0 + 9 * i] = fv0[i0 + 9 * i] + b_r[i0 + 9 * i];
+ }
+ }
+
+ mrdivide(b_P_apriori, fv1, K_k);
+
+ /* 'attitudeKalmanfilter:136' x_aposteriori=x_apriori+K_k*y_k; */
+ for (i = 0; i < 9; i++) {
+ f0 = 0.0F;
+ for (i0 = 0; i0 < 12; i0++) {
+ f0 += (real32_T)iv2[i + 9 * i0] * x_apriori[i0];
+ }
+
+ O[i] = z[i] - f0;
+ }
+
+ for (i = 0; i < 12; i++) {
+ f0 = 0.0F;
+ for (i0 = 0; i0 < 9; i0++) {
+ f0 += K_k[i + 12 * i0] * O[i0];
+ }
+
+ x_aposteriori[i] = x_apriori[i] + f0;
+ }
+
+ /* 'attitudeKalmanfilter:137' 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++) {
+ f0 = 0.0F;
+ for (i1 = 0; i1 < 9; i1++) {
+ f0 += K_k[i + 12 * i1] * (real32_T)iv2[i1 + 9 * i0];
+ }
+
+ b_A_lin[i + 12 * i0] = (real32_T)dv1[i + 12 * i0] - f0;
+ }
+ }
+
+ for (i = 0; i < 12; i++) {
+ for (i0 = 0; i0 < 12; i0++) {
+ P_aposteriori[i + 12 * i0] = 0.0F;
+ for (i1 = 0; i1 < 12; i1++) {
+ P_aposteriori[i + 12 * i0] += b_A_lin[i + 12 * i1] * P_apriori[i1 + 12
+ * i0];
+ }
+ }
+ }
+ } else {
+ /* 'attitudeKalmanfilter:138' else */
+ /* 'attitudeKalmanfilter:139' if updateVect(1)==1&&updateVect(2)==0&&updateVect(3)==0 */
+ if ((updateVect[0] == 1) && (updateVect[1] == 0) && (updateVect[2] == 0)) {
+ /* 'attitudeKalmanfilter:141' R=[r(1),0,0; */
+ /* 'attitudeKalmanfilter:142' 0,r(1),0; */
+ /* 'attitudeKalmanfilter:143' 0,0,r(1)]; */
+ /* observation matrix */
+ /* 'attitudeKalmanfilter:146' H_k=[ E, Z, Z, Z]; */
+ /* 'attitudeKalmanfilter:148' y_k=z(1:3)-H_k(1:3,1:12)*x_apriori; */
+ /* 'attitudeKalmanfilter:150' S_k=H_k(1:3,1:12)*P_apriori*H_k(1:3,1:12)'+R(1:3,1:3); */
+ /* 'attitudeKalmanfilter:151' 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;
+ for (i1 = 0; i1 < 12; i1++) {
+ c_P_apriori[i + 12 * i0] += P_apriori[i + 12 * i1] * (real32_T)
+ iv3[i1 + 12 * i0];
+ }
+ }
+ }
+
+ for (i = 0; i < 3; i++) {
+ for (i0 = 0; i0 < 12; i0++) {
+ fv2[i + 3 * i0] = 0.0F;
+ for (i1 = 0; i1 < 12; i1++) {
+ fv2[i + 3 * i0] += (real32_T)iv4[i + 3 * i1] * P_apriori[i1 + 12 *
+ i0];
+ }
+ }
+
+ for (i0 = 0; i0 < 3; i0++) {
+ O[i + 3 * i0] = 0.0F;
+ for (i1 = 0; i1 < 12; i1++) {
+ O[i + 3 * i0] += fv2[i + 3 * i1] * (real32_T)iv3[i1 + 12 * i0];
+ }
+ }
+ }
+
+ c_r[0] = r[0];
+ c_r[3] = 0.0F;
+ c_r[6] = 0.0F;
+ c_r[1] = 0.0F;
+ c_r[4] = r[0];
+ c_r[7] = 0.0F;
+ c_r[2] = 0.0F;
+ c_r[5] = 0.0F;
+ c_r[8] = r[0];
+ for (i = 0; i < 3; i++) {
+ for (i0 = 0; i0 < 3; i0++) {
+ a[i0 + 3 * i] = O[i0 + 3 * i] + c_r[i0 + 3 * i];
+ }
+ }
+
+ b_mrdivide(c_P_apriori, a, b_K_k);
+
+ /* 'attitudeKalmanfilter:154' x_aposteriori=x_apriori+K_k*y_k; */
+ for (i = 0; i < 3; i++) {
+ f0 = 0.0F;
+ for (i0 = 0; i0 < 12; i0++) {
+ f0 += (real32_T)iv4[i + 3 * i0] * x_apriori[i0];
+ }
+
+ x_n_b[i] = z[i] - f0;
+ }
+
+ for (i = 0; i < 12; i++) {
+ f0 = 0.0F;
+ for (i0 = 0; i0 < 3; i0++) {
+ f0 += b_K_k[i + 12 * i0] * x_n_b[i0];
+ }
+
+ x_aposteriori[i] = x_apriori[i] + f0;
+ }
+
+ /* 'attitudeKalmanfilter:155' 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++) {
+ f0 = 0.0F;
+ for (i1 = 0; i1 < 3; i1++) {
+ f0 += b_K_k[i + 12 * i1] * (real32_T)iv4[i1 + 3 * i0];
+ }
+
+ b_A_lin[i + 12 * i0] = (real32_T)dv1[i + 12 * i0] - f0;
+ }
+ }
+
+ for (i = 0; i < 12; i++) {
+ for (i0 = 0; i0 < 12; i0++) {
+ P_aposteriori[i + 12 * i0] = 0.0F;
+ for (i1 = 0; i1 < 12; i1++) {
+ P_aposteriori[i + 12 * i0] += b_A_lin[i + 12 * i1] * P_apriori[i1 +
+ 12 * i0];
+ }
+ }
+ }
+ } else {
+ /* 'attitudeKalmanfilter:156' else */
+ /* 'attitudeKalmanfilter:157' if updateVect(1)==1&&updateVect(2)==1&&updateVect(3)==0 */
+ if ((updateVect[0] == 1) && (updateVect[1] == 1) && (updateVect[2] == 0))
+ {
+ /* 'attitudeKalmanfilter:158' if z(6)<4 || z(5)>15 */
+ if ((z[5] < 4.0F) || (z[4] > 15.0F)) {
+ /* 'attitudeKalmanfilter:159' r(2)=10000; */
+ r[1] = 10000.0F;
+ }
+
+ /* 'attitudeKalmanfilter:162' R=[r(1),0,0,0,0,0; */
+ /* 'attitudeKalmanfilter:163' 0,r(1),0,0,0,0; */
+ /* 'attitudeKalmanfilter:164' 0,0,r(1),0,0,0; */
+ /* 'attitudeKalmanfilter:165' 0,0,0,r(2),0,0; */
+ /* 'attitudeKalmanfilter:166' 0,0,0,0,r(2),0; */
+ /* 'attitudeKalmanfilter:167' 0,0,0,0,0,r(2)]; */
+ /* observation matrix */
+ /* 'attitudeKalmanfilter:170' H_k=[ E, Z, Z, Z; */
+ /* 'attitudeKalmanfilter:171' Z, Z, E, Z]; */
+ /* 'attitudeKalmanfilter:173' y_k=z(1:6)-H_k(1:6,1:12)*x_apriori; */
+ /* 'attitudeKalmanfilter:175' S_k=H_k(1:6,1:12)*P_apriori*H_k(1:6,1:12)'+R(1:6,1:6); */
+ /* 'attitudeKalmanfilter:176' 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;
+ for (i1 = 0; i1 < 12; i1++) {
+ d_P_apriori[i + 12 * i0] += P_apriori[i + 12 * i1] * (real32_T)
+ iv5[i1 + 12 * i0];
+ }
+ }
+ }
+
+ for (i = 0; i < 6; i++) {
+ for (i0 = 0; i0 < 12; i0++) {
+ c_K_k[i + 6 * i0] = 0.0F;
+ for (i1 = 0; i1 < 12; i1++) {
+ c_K_k[i + 6 * i0] += (real32_T)iv6[i + 6 * i1] * P_apriori[i1 + 12
+ * i0];
+ }
+ }
+
+ for (i0 = 0; i0 < 6; i0++) {
+ fv2[i + 6 * i0] = 0.0F;
+ for (i1 = 0; i1 < 12; i1++) {
+ fv2[i + 6 * i0] += c_K_k[i + 6 * i1] * (real32_T)iv5[i1 + 12 * i0];
+ }
+ }
+ }
+
+ b_K_k[0] = r[0];
+ b_K_k[6] = 0.0F;
+ b_K_k[12] = 0.0F;
+ b_K_k[18] = 0.0F;
+ b_K_k[24] = 0.0F;
+ b_K_k[30] = 0.0F;
+ b_K_k[1] = 0.0F;
+ b_K_k[7] = r[0];
+ b_K_k[13] = 0.0F;
+ b_K_k[19] = 0.0F;
+ b_K_k[25] = 0.0F;
+ b_K_k[31] = 0.0F;
+ b_K_k[2] = 0.0F;
+ b_K_k[8] = 0.0F;
+ b_K_k[14] = r[0];
+ b_K_k[20] = 0.0F;
+ b_K_k[26] = 0.0F;
+ b_K_k[32] = 0.0F;
+ b_K_k[3] = 0.0F;
+ b_K_k[9] = 0.0F;
+ b_K_k[15] = 0.0F;
+ b_K_k[21] = r[1];
+ b_K_k[27] = 0.0F;
+ b_K_k[33] = 0.0F;
+ b_K_k[4] = 0.0F;
+ b_K_k[10] = 0.0F;
+ b_K_k[16] = 0.0F;
+ b_K_k[22] = 0.0F;
+ b_K_k[28] = r[1];
+ b_K_k[34] = 0.0F;
+ b_K_k[5] = 0.0F;
+ b_K_k[11] = 0.0F;
+ b_K_k[17] = 0.0F;
+ b_K_k[23] = 0.0F;
+ b_K_k[29] = 0.0F;
+ b_K_k[35] = r[1];
+ for (i = 0; i < 6; i++) {
+ for (i0 = 0; i0 < 6; i0++) {
+ c_P_apriori[i0 + 6 * i] = fv2[i0 + 6 * i] + b_K_k[i0 + 6 * i];
+ }
+ }
+
+ c_mrdivide(d_P_apriori, c_P_apriori, c_K_k);
+
+ /* 'attitudeKalmanfilter:179' x_aposteriori=x_apriori+K_k*y_k; */
+ for (i = 0; i < 6; i++) {
+ f0 = 0.0F;
+ for (i0 = 0; i0 < 12; i0++) {
+ f0 += (real32_T)iv6[i + 6 * i0] * x_apriori[i0];
+ }
+
+ b_z[i] = z[i] - f0;
+ }
+
+ for (i = 0; i < 12; i++) {
+ f0 = 0.0F;
+ for (i0 = 0; i0 < 6; i0++) {
+ f0 += c_K_k[i + 12 * i0] * b_z[i0];
+ }
+
+ x_aposteriori[i] = x_apriori[i] + f0;
+ }
+
+ /* 'attitudeKalmanfilter:180' 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++) {
+ f0 = 0.0F;
+ for (i1 = 0; i1 < 6; i1++) {
+ f0 += c_K_k[i + 12 * i1] * (real32_T)iv6[i1 + 6 * i0];
+ }
+
+ b_A_lin[i + 12 * i0] = (real32_T)dv1[i + 12 * i0] - f0;
+ }
+ }
+
+ for (i = 0; i < 12; i++) {
+ for (i0 = 0; i0 < 12; i0++) {
+ P_aposteriori[i + 12 * i0] = 0.0F;
+ for (i1 = 0; i1 < 12; i1++) {
+ P_aposteriori[i + 12 * i0] += b_A_lin[i + 12 * i1] * P_apriori[i1
+ + 12 * i0];
+ }
+ }
+ }
+ } else {
+ /* 'attitudeKalmanfilter:181' else */
+ /* 'attitudeKalmanfilter:182' if updateVect(1)==1&&updateVect(2)==0&&updateVect(3)==1 */
+ if ((updateVect[0] == 1) && (updateVect[1] == 0) && (updateVect[2] == 1))
+ {
+ /* 'attitudeKalmanfilter:183' R=[r(1),0,0,0,0,0; */
+ /* 'attitudeKalmanfilter:184' 0,r(1),0,0,0,0; */
+ /* 'attitudeKalmanfilter:185' 0,0,r(1),0,0,0; */
+ /* 'attitudeKalmanfilter:186' 0,0,0,r(3),0,0; */
+ /* 'attitudeKalmanfilter:187' 0,0,0,0,r(3),0; */
+ /* 'attitudeKalmanfilter:188' 0,0,0,0,0,r(3)]; */
+ /* observation matrix */
+ /* 'attitudeKalmanfilter:191' H_k=[ E, Z, Z, Z; */
+ /* 'attitudeKalmanfilter:192' Z, Z, Z, E]; */
+ /* 'attitudeKalmanfilter:194' y_k=[z(1:3);z(7:9)]-H_k(1:6,1:12)*x_apriori; */
+ /* 'attitudeKalmanfilter:196' S_k=H_k(1:6,1:12)*P_apriori*H_k(1:6,1:12)'+R(1:6,1:6); */
+ /* 'attitudeKalmanfilter:197' 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;
+ for (i1 = 0; i1 < 12; i1++) {
+ d_P_apriori[i + 12 * i0] += P_apriori[i + 12 * i1] * (real32_T)
+ iv7[i1 + 12 * i0];
+ }
+ }
+ }
+
+ for (i = 0; i < 6; i++) {
+ for (i0 = 0; i0 < 12; i0++) {
+ c_K_k[i + 6 * i0] = 0.0F;
+ for (i1 = 0; i1 < 12; i1++) {
+ c_K_k[i + 6 * i0] += (real32_T)iv8[i + 6 * i1] * P_apriori[i1 +
+ 12 * i0];
+ }
+ }
+
+ for (i0 = 0; i0 < 6; i0++) {
+ fv2[i + 6 * i0] = 0.0F;
+ for (i1 = 0; i1 < 12; i1++) {
+ fv2[i + 6 * i0] += c_K_k[i + 6 * i1] * (real32_T)iv7[i1 + 12 *
+ i0];
+ }
+ }
+ }
+
+ b_K_k[0] = r[0];
+ b_K_k[6] = 0.0F;
+ b_K_k[12] = 0.0F;
+ b_K_k[18] = 0.0F;
+ b_K_k[24] = 0.0F;
+ b_K_k[30] = 0.0F;
+ b_K_k[1] = 0.0F;
+ b_K_k[7] = r[0];
+ b_K_k[13] = 0.0F;
+ b_K_k[19] = 0.0F;
+ b_K_k[25] = 0.0F;
+ b_K_k[31] = 0.0F;
+ b_K_k[2] = 0.0F;
+ b_K_k[8] = 0.0F;
+ b_K_k[14] = r[0];
+ b_K_k[20] = 0.0F;
+ b_K_k[26] = 0.0F;
+ b_K_k[32] = 0.0F;
+ b_K_k[3] = 0.0F;
+ b_K_k[9] = 0.0F;
+ b_K_k[15] = 0.0F;
+ b_K_k[21] = r[2];
+ b_K_k[27] = 0.0F;
+ b_K_k[33] = 0.0F;
+ b_K_k[4] = 0.0F;
+ b_K_k[10] = 0.0F;
+ b_K_k[16] = 0.0F;
+ b_K_k[22] = 0.0F;
+ b_K_k[28] = r[2];
+ b_K_k[34] = 0.0F;
+ b_K_k[5] = 0.0F;
+ b_K_k[11] = 0.0F;
+ b_K_k[17] = 0.0F;
+ b_K_k[23] = 0.0F;
+ b_K_k[29] = 0.0F;
+ b_K_k[35] = r[2];
+ for (i = 0; i < 6; i++) {
+ for (i0 = 0; i0 < 6; i0++) {
+ c_P_apriori[i0 + 6 * i] = fv2[i0 + 6 * i] + b_K_k[i0 + 6 * i];
+ }
+ }
+
+ c_mrdivide(d_P_apriori, c_P_apriori, c_K_k);
+
+ /* 'attitudeKalmanfilter:200' x_aposteriori=x_apriori+K_k*y_k; */
+ for (i = 0; i < 3; i++) {
+ b_z[i] = z[i];
+ }
+
+ for (i = 0; i < 3; i++) {
+ b_z[i + 3] = z[i + 6];
+ }
+
+ for (i = 0; i < 6; i++) {
+ fv3[i] = 0.0F;
+ for (i0 = 0; i0 < 12; i0++) {
+ fv3[i] += (real32_T)iv8[i + 6 * i0] * x_apriori[i0];
+ }
+
+ c_z[i] = b_z[i] - fv3[i];
+ }
+
+ for (i = 0; i < 12; i++) {
+ f0 = 0.0F;
+ for (i0 = 0; i0 < 6; i0++) {
+ f0 += c_K_k[i + 12 * i0] * c_z[i0];
+ }
+
+ x_aposteriori[i] = x_apriori[i] + f0;
+ }
+
+ /* 'attitudeKalmanfilter:201' 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++) {
+ f0 = 0.0F;
+ for (i1 = 0; i1 < 6; i1++) {
+ f0 += c_K_k[i + 12 * i1] * (real32_T)iv8[i1 + 6 * i0];
+ }
+
+ b_A_lin[i + 12 * i0] = (real32_T)dv1[i + 12 * i0] - f0;
+ }
+ }
+
+ for (i = 0; i < 12; i++) {
+ for (i0 = 0; i0 < 12; i0++) {
+ P_aposteriori[i + 12 * i0] = 0.0F;
+ for (i1 = 0; i1 < 12; i1++) {
+ P_aposteriori[i + 12 * i0] += b_A_lin[i + 12 * i1] *
+ P_apriori[i1 + 12 * i0];
+ }
+ }
+ }
+ } else {
+ /* 'attitudeKalmanfilter:202' else */
+ /* 'attitudeKalmanfilter:203' x_aposteriori=x_apriori; */
+ for (i = 0; i < 12; i++) {
+ x_aposteriori[i] = x_apriori[i];
+ }
+
+ /* 'attitudeKalmanfilter:204' P_aposteriori=P_apriori; */
+ memcpy(&P_aposteriori[0], &P_apriori[0], 144U * sizeof(real32_T));
+ }
+ }
+ }
+ }
+
+ /* % euler anglels extraction */
+ /* 'attitudeKalmanfilter:213' 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];
+ }
+
+ rdivide(x_n_b, norm(*(real32_T (*)[3])&x_aposteriori[6]), z_n_b);
+
+ /* 'attitudeKalmanfilter:214' 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]), wak);
+
+ /* 'attitudeKalmanfilter:216' y_n_b=cross(z_n_b,m_n_b); */
+ for (i = 0; i < 3; i++) {
+ x_n_b[i] = wak[i];
+ }
+
+ cross(z_n_b, x_n_b, wak);
+
+ /* 'attitudeKalmanfilter:217' y_n_b=y_n_b./norm(y_n_b); */
+ for (i = 0; i < 3; i++) {
+ x_n_b[i] = wak[i];
+ }
+
+ rdivide(x_n_b, norm(wak), wak);
+
+ /* 'attitudeKalmanfilter:219' x_n_b=(cross(y_n_b,z_n_b)); */
+ cross(wak, z_n_b, x_n_b);
+
+ /* 'attitudeKalmanfilter:220' x_n_b=x_n_b./norm(x_n_b); */
+ for (i = 0; i < 3; i++) {
+ b_x_aposteriori_k[i] = x_n_b[i];
+ }
+
+ rdivide(b_x_aposteriori_k, norm(x_n_b), x_n_b);
+
+ /* 'attitudeKalmanfilter:226' Rot_matrix=[x_n_b,y_n_b,z_n_b]; */
+ for (i = 0; i < 3; i++) {
+ Rot_matrix[i] = x_n_b[i];
+ Rot_matrix[3 + i] = wak[i];
+ Rot_matrix[6 + i] = z_n_b[i];
+ }
+
+ /* 'attitudeKalmanfilter:230' phi=atan2(Rot_matrix(2,3),Rot_matrix(3,3)); */
+ /* 'attitudeKalmanfilter:231' theta=-asin(Rot_matrix(1,3)); */
+ /* 'attitudeKalmanfilter:232' psi=atan2(Rot_matrix(1,2),Rot_matrix(1,1)); */
+ /* 'attitudeKalmanfilter:233' eulerAngles=[phi;theta;psi]; */
+ eulerAngles[0] = rt_atan2f_snf(Rot_matrix[7], Rot_matrix[8]);
+ eulerAngles[1] = -(real32_T)asin(Rot_matrix[6]);
+ eulerAngles[2] = rt_atan2f_snf(Rot_matrix[3], Rot_matrix[0]);
+}
+
+/* End of code generation (attitudeKalmanfilter.c) */
diff --git a/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter.h b/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter.h new file mode 100755 index 000000000..afa63c1a9 --- /dev/null +++ b/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter.h @@ -0,0 +1,34 @@ +/*
+ * attitudeKalmanfilter.h
+ *
+ * Code generation for function 'attitudeKalmanfilter'
+ *
+ * C source code generated on: Sat Jan 19 15:25:29 2013
+ *
+ */
+
+#ifndef __ATTITUDEKALMANFILTER_H__
+#define __ATTITUDEKALMANFILTER_H__
+/* Include files */
+#include <math.h>
+#include <stddef.h>
+#include <stdlib.h>
+#include <string.h>
+#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 attitudeKalmanfilter(const uint8_T updateVect[3], real32_T dt, const real32_T z[9], const real32_T x_aposteriori_k[12], const real32_T P_aposteriori_k[144], const real32_T q[12], real32_T r[9], real32_T eulerAngles[3], real32_T Rot_matrix[9], real32_T x_aposteriori[12], real32_T P_aposteriori[144]);
+#endif
+/* End of code generation (attitudeKalmanfilter.h) */
diff --git a/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.c b/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.c new file mode 100755 index 000000000..7d620d7fa --- /dev/null +++ b/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.c @@ -0,0 +1,31 @@ +/*
+ * attitudeKalmanfilter_initialize.c
+ *
+ * Code generation for function 'attitudeKalmanfilter_initialize'
+ *
+ * C source code generated on: Sat Jan 19 15:25:29 2013
+ *
+ */
+
+/* Include files */
+#include "rt_nonfinite.h"
+#include "attitudeKalmanfilter.h"
+#include "attitudeKalmanfilter_initialize.h"
+
+/* Type Definitions */
+
+/* Named Constants */
+
+/* Variable Declarations */
+
+/* Variable Definitions */
+
+/* Function Declarations */
+
+/* Function Definitions */
+void attitudeKalmanfilter_initialize(void)
+{
+ rt_InitInfAndNaN(8U);
+}
+
+/* End of code generation (attitudeKalmanfilter_initialize.c) */
diff --git a/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.h b/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.h new file mode 100755 index 000000000..8b599be66 --- /dev/null +++ b/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.h @@ -0,0 +1,34 @@ +/*
+ * attitudeKalmanfilter_initialize.h
+ *
+ * Code generation for function 'attitudeKalmanfilter_initialize'
+ *
+ * C source code generated on: Sat Jan 19 15:25:29 2013
+ *
+ */
+
+#ifndef __ATTITUDEKALMANFILTER_INITIALIZE_H__
+#define __ATTITUDEKALMANFILTER_INITIALIZE_H__
+/* Include files */
+#include <math.h>
+#include <stddef.h>
+#include <stdlib.h>
+#include <string.h>
+#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 attitudeKalmanfilter_initialize(void);
+#endif
+/* End of code generation (attitudeKalmanfilter_initialize.h) */
diff --git a/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.c b/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.c new file mode 100755 index 000000000..7f9727419 --- /dev/null +++ b/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.c @@ -0,0 +1,31 @@ +/*
+ * attitudeKalmanfilter_terminate.c
+ *
+ * Code generation for function 'attitudeKalmanfilter_terminate'
+ *
+ * C source code generated on: Sat Jan 19 15:25:29 2013
+ *
+ */
+
+/* Include files */
+#include "rt_nonfinite.h"
+#include "attitudeKalmanfilter.h"
+#include "attitudeKalmanfilter_terminate.h"
+
+/* Type Definitions */
+
+/* Named Constants */
+
+/* Variable Declarations */
+
+/* Variable Definitions */
+
+/* Function Declarations */
+
+/* Function Definitions */
+void attitudeKalmanfilter_terminate(void)
+{
+ /* (no terminate code required) */
+}
+
+/* End of code generation (attitudeKalmanfilter_terminate.c) */
diff --git a/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.h b/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.h new file mode 100755 index 000000000..da84a5024 --- /dev/null +++ b/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.h @@ -0,0 +1,34 @@ +/*
+ * attitudeKalmanfilter_terminate.h
+ *
+ * Code generation for function 'attitudeKalmanfilter_terminate'
+ *
+ * C source code generated on: Sat Jan 19 15:25:29 2013
+ *
+ */
+
+#ifndef __ATTITUDEKALMANFILTER_TERMINATE_H__
+#define __ATTITUDEKALMANFILTER_TERMINATE_H__
+/* Include files */
+#include <math.h>
+#include <stddef.h>
+#include <stdlib.h>
+#include <string.h>
+#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 attitudeKalmanfilter_terminate(void);
+#endif
+/* End of code generation (attitudeKalmanfilter_terminate.h) */
diff --git a/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_types.h b/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_types.h new file mode 100755 index 000000000..30fd1e75e --- /dev/null +++ b/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_types.h @@ -0,0 +1,16 @@ +/*
+ * attitudeKalmanfilter_types.h
+ *
+ * Code generation for function 'attitudeKalmanfilter'
+ *
+ * C source code generated on: Sat Jan 19 15:25:29 2013
+ *
+ */
+
+#ifndef __ATTITUDEKALMANFILTER_TYPES_H__
+#define __ATTITUDEKALMANFILTER_TYPES_H__
+
+/* Type Definitions */
+
+#endif
+/* End of code generation (attitudeKalmanfilter_types.h) */
diff --git a/src/modules/attitude_estimator_ekf/codegen/cross.c b/src/modules/attitude_estimator_ekf/codegen/cross.c new file mode 100755 index 000000000..84ada9002 --- /dev/null +++ b/src/modules/attitude_estimator_ekf/codegen/cross.c @@ -0,0 +1,37 @@ +/*
+ * cross.c
+ *
+ * Code generation for function 'cross'
+ *
+ * C source code generated on: Sat Jan 19 15:25:29 2013
+ *
+ */
+
+/* Include files */
+#include "rt_nonfinite.h"
+#include "attitudeKalmanfilter.h"
+#include "cross.h"
+
+/* Type Definitions */
+
+/* Named Constants */
+
+/* Variable Declarations */
+
+/* Variable Definitions */
+
+/* Function Declarations */
+
+/* Function Definitions */
+
+/*
+ *
+ */
+void cross(const real32_T a[3], const real32_T b[3], real32_T c[3])
+{
+ c[0] = a[1] * b[2] - a[2] * b[1];
+ c[1] = a[2] * b[0] - a[0] * b[2];
+ c[2] = a[0] * b[1] - a[1] * b[0];
+}
+
+/* End of code generation (cross.c) */
diff --git a/src/modules/attitude_estimator_ekf/codegen/cross.h b/src/modules/attitude_estimator_ekf/codegen/cross.h new file mode 100755 index 000000000..e727f0684 --- /dev/null +++ b/src/modules/attitude_estimator_ekf/codegen/cross.h @@ -0,0 +1,34 @@ +/*
+ * cross.h
+ *
+ * Code generation for function 'cross'
+ *
+ * C source code generated on: Sat Jan 19 15:25:29 2013
+ *
+ */
+
+#ifndef __CROSS_H__
+#define __CROSS_H__
+/* Include files */
+#include <math.h>
+#include <stddef.h>
+#include <stdlib.h>
+#include <string.h>
+#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 cross(const real32_T a[3], const real32_T b[3], real32_T c[3]);
+#endif
+/* End of code generation (cross.h) */
diff --git a/src/modules/attitude_estimator_ekf/codegen/eye.c b/src/modules/attitude_estimator_ekf/codegen/eye.c new file mode 100755 index 000000000..b89ab58ef --- /dev/null +++ b/src/modules/attitude_estimator_ekf/codegen/eye.c @@ -0,0 +1,51 @@ +/*
+ * eye.c
+ *
+ * Code generation for function 'eye'
+ *
+ * C source code generated on: Sat Jan 19 15:25:29 2013
+ *
+ */
+
+/* Include files */
+#include "rt_nonfinite.h"
+#include "attitudeKalmanfilter.h"
+#include "eye.h"
+
+/* Type Definitions */
+
+/* Named Constants */
+
+/* Variable Declarations */
+
+/* Variable Definitions */
+
+/* Function Declarations */
+
+/* Function Definitions */
+
+/*
+ *
+ */
+void b_eye(real_T I[144])
+{
+ int32_T i;
+ memset(&I[0], 0, 144U * sizeof(real_T));
+ for (i = 0; i < 12; i++) {
+ I[i + 12 * i] = 1.0;
+ }
+}
+
+/*
+ *
+ */
+void eye(real_T I[9])
+{
+ int32_T i;
+ memset(&I[0], 0, 9U * sizeof(real_T));
+ for (i = 0; i < 3; i++) {
+ I[i + 3 * i] = 1.0;
+ }
+}
+
+/* End of code generation (eye.c) */
diff --git a/src/modules/attitude_estimator_ekf/codegen/eye.h b/src/modules/attitude_estimator_ekf/codegen/eye.h new file mode 100755 index 000000000..94fbe7671 --- /dev/null +++ b/src/modules/attitude_estimator_ekf/codegen/eye.h @@ -0,0 +1,35 @@ +/*
+ * eye.h
+ *
+ * Code generation for function 'eye'
+ *
+ * C source code generated on: Sat Jan 19 15:25:29 2013
+ *
+ */
+
+#ifndef __EYE_H__
+#define __EYE_H__
+/* Include files */
+#include <math.h>
+#include <stddef.h>
+#include <stdlib.h>
+#include <string.h>
+#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 b_eye(real_T I[144]);
+extern void eye(real_T I[9]);
+#endif
+/* End of code generation (eye.h) */
diff --git a/src/modules/attitude_estimator_ekf/codegen/mrdivide.c b/src/modules/attitude_estimator_ekf/codegen/mrdivide.c new file mode 100755 index 000000000..a810f22e4 --- /dev/null +++ b/src/modules/attitude_estimator_ekf/codegen/mrdivide.c @@ -0,0 +1,357 @@ +/*
+ * mrdivide.c
+ *
+ * Code generation for function 'mrdivide'
+ *
+ * C source code generated on: Sat Jan 19 15:25:29 2013
+ *
+ */
+
+/* Include files */
+#include "rt_nonfinite.h"
+#include "attitudeKalmanfilter.h"
+#include "mrdivide.h"
+
+/* Type Definitions */
+
+/* Named Constants */
+
+/* Variable Declarations */
+
+/* Variable Definitions */
+
+/* Function Declarations */
+
+/* Function Definitions */
+
+/*
+ *
+ */
+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_B[36];
+ int32_T r1;
+ int32_T r2;
+ int32_T r3;
+ real32_T maxval;
+ real32_T a21;
+ real32_T Y[36];
+ for (rtemp = 0; rtemp < 3; rtemp++) {
+ for (k = 0; k < 3; k++) {
+ b_A[k + 3 * rtemp] = B[rtemp + 3 * k];
+ }
+ }
+
+ for (rtemp = 0; rtemp < 12; rtemp++) {
+ for (k = 0; k < 3; k++) {
+ b_B[k + 3 * rtemp] = A[rtemp + 12 * k];
+ }
+ }
+
+ r1 = 0;
+ r2 = 1;
+ r3 = 2;
+ 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)fabs(b_A[2]) > maxval) {
+ r1 = 2;
+ r2 = 1;
+ r3 = 0;
+ }
+
+ b_A[r2] /= b_A[r1];
+ b_A[r3] /= b_A[r1];
+ b_A[3 + r2] -= b_A[r2] * b_A[3 + r1];
+ 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)fabs(b_A[3 + r3]) > (real32_T)fabs(b_A[3 + r2])) {
+ rtemp = r2;
+ r2 = r3;
+ r3 = rtemp;
+ }
+
+ b_A[3 + r3] /= b_A[3 + r2];
+ b_A[6 + r3] -= b_A[3 + r3] * b_A[6 + r2];
+ for (k = 0; k < 12; k++) {
+ Y[3 * k] = b_B[r1 + 3 * k];
+ Y[1 + 3 * k] = b_B[r2 + 3 * k] - Y[3 * k] * b_A[r2];
+ Y[2 + 3 * k] = (b_B[r3 + 3 * k] - Y[3 * k] * b_A[r3]) - Y[1 + 3 * k] * b_A[3
+ + r3];
+ Y[2 + 3 * k] /= b_A[6 + r3];
+ Y[3 * k] -= Y[2 + 3 * k] * b_A[6 + r1];
+ Y[1 + 3 * k] -= Y[2 + 3 * k] * b_A[6 + r2];
+ Y[1 + 3 * k] /= b_A[3 + r2];
+ Y[3 * k] -= Y[1 + 3 * k] * b_A[3 + r1];
+ Y[3 * k] /= b_A[r1];
+ }
+
+ for (rtemp = 0; rtemp < 3; rtemp++) {
+ for (k = 0; k < 12; k++) {
+ y[k + 12 * rtemp] = Y[rtemp + 3 * k];
+ }
+ }
+}
+
+/*
+ *
+ */
+void c_mrdivide(const real32_T A[72], const real32_T B[36], real32_T y[72])
+{
+ real32_T b_A[36];
+ int8_T ipiv[6];
+ int32_T i3;
+ int32_T iy;
+ int32_T j;
+ int32_T c;
+ int32_T ix;
+ real32_T temp;
+ int32_T k;
+ real32_T s;
+ int32_T jy;
+ int32_T ijA;
+ real32_T Y[72];
+ for (i3 = 0; i3 < 6; i3++) {
+ for (iy = 0; iy < 6; iy++) {
+ b_A[iy + 6 * i3] = B[i3 + 6 * iy];
+ }
+
+ ipiv[i3] = (int8_T)(1 + i3);
+ }
+
+ for (j = 0; j < 5; j++) {
+ 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)fabs(b_A[ix]);
+ if (s > temp) {
+ iy = k - 1;
+ temp = s;
+ }
+ }
+
+ if (b_A[c + iy] != 0.0F) {
+ if (iy != 0) {
+ ipiv[j] = (int8_T)((j + iy) + 1);
+ ix = j;
+ iy += j;
+ for (k = 0; k < 6; k++) {
+ temp = b_A[ix];
+ b_A[ix] = b_A[iy];
+ b_A[iy] = temp;
+ ix += 6;
+ iy += 6;
+ }
+ }
+
+ i3 = (c - j) + 6;
+ for (jy = c + 1; jy + 1 <= i3; jy++) {
+ b_A[jy] /= b_A[c];
+ }
+ }
+
+ 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;
+ iy += 6;
+ }
+ }
+
+ for (i3 = 0; i3 < 12; i3++) {
+ for (iy = 0; iy < 6; iy++) {
+ Y[iy + 6 * i3] = A[i3 + 12 * iy];
+ }
+ }
+
+ for (jy = 0; jy < 6; jy++) {
+ if (ipiv[jy] != jy + 1) {
+ for (j = 0; j < 12; j++) {
+ temp = Y[jy + 6 * j];
+ Y[jy + 6 * j] = Y[(ipiv[jy] + 6 * j) - 1];
+ Y[(ipiv[jy] + 6 * j) - 1] = temp;
+ }
+ }
+ }
+
+ for (j = 0; j < 12; j++) {
+ c = 6 * j;
+ for (k = 0; k < 6; k++) {
+ 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];
+ }
+ }
+ }
+ }
+
+ for (j = 0; j < 12; j++) {
+ c = 6 * j;
+ for (k = 5; k > -1; k += -1) {
+ 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 (i3 = 0; i3 < 6; i3++) {
+ for (iy = 0; iy < 12; iy++) {
+ y[iy + 12 * i3] = Y[i3 + 6 * iy];
+ }
+ }
+}
+
+/*
+ *
+ */
+void mrdivide(const real32_T A[108], const real32_T B[81], real32_T y[108])
+{
+ real32_T b_A[81];
+ int8_T ipiv[9];
+ int32_T i2;
+ int32_T iy;
+ int32_T j;
+ int32_T c;
+ int32_T ix;
+ real32_T temp;
+ int32_T k;
+ real32_T s;
+ int32_T jy;
+ int32_T ijA;
+ real32_T Y[108];
+ for (i2 = 0; i2 < 9; i2++) {
+ for (iy = 0; iy < 9; iy++) {
+ b_A[iy + 9 * i2] = B[i2 + 9 * iy];
+ }
+
+ ipiv[i2] = (int8_T)(1 + i2);
+ }
+
+ for (j = 0; j < 8; j++) {
+ 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)fabs(b_A[ix]);
+ if (s > temp) {
+ iy = k - 1;
+ temp = s;
+ }
+ }
+
+ if (b_A[c + iy] != 0.0F) {
+ if (iy != 0) {
+ ipiv[j] = (int8_T)((j + iy) + 1);
+ ix = j;
+ iy += j;
+ for (k = 0; k < 9; k++) {
+ temp = b_A[ix];
+ b_A[ix] = b_A[iy];
+ b_A[iy] = temp;
+ ix += 9;
+ iy += 9;
+ }
+ }
+
+ i2 = (c - j) + 9;
+ for (jy = c + 1; jy + 1 <= i2; jy++) {
+ b_A[jy] /= b_A[c];
+ }
+ }
+
+ 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;
+ iy += 9;
+ }
+ }
+
+ for (i2 = 0; i2 < 12; i2++) {
+ for (iy = 0; iy < 9; iy++) {
+ Y[iy + 9 * i2] = A[i2 + 12 * iy];
+ }
+ }
+
+ for (jy = 0; jy < 9; jy++) {
+ if (ipiv[jy] != jy + 1) {
+ for (j = 0; j < 12; j++) {
+ temp = Y[jy + 9 * j];
+ Y[jy + 9 * j] = Y[(ipiv[jy] + 9 * j) - 1];
+ Y[(ipiv[jy] + 9 * j) - 1] = temp;
+ }
+ }
+ }
+
+ for (j = 0; j < 12; j++) {
+ c = 9 * j;
+ for (k = 0; k < 9; k++) {
+ 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];
+ }
+ }
+ }
+ }
+
+ for (j = 0; j < 12; j++) {
+ c = 9 * j;
+ for (k = 8; k > -1; k += -1) {
+ 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 (i2 = 0; i2 < 9; i2++) {
+ for (iy = 0; iy < 12; iy++) {
+ y[iy + 12 * i2] = Y[i2 + 9 * iy];
+ }
+ }
+}
+
+/* End of code generation (mrdivide.c) */
diff --git a/src/modules/attitude_estimator_ekf/codegen/mrdivide.h b/src/modules/attitude_estimator_ekf/codegen/mrdivide.h new file mode 100755 index 000000000..2d3b0d51f --- /dev/null +++ b/src/modules/attitude_estimator_ekf/codegen/mrdivide.h @@ -0,0 +1,36 @@ +/*
+ * mrdivide.h
+ *
+ * Code generation for function 'mrdivide'
+ *
+ * C source code generated on: Sat Jan 19 15:25:29 2013
+ *
+ */
+
+#ifndef __MRDIVIDE_H__
+#define __MRDIVIDE_H__
+/* Include files */
+#include <math.h>
+#include <stddef.h>
+#include <stdlib.h>
+#include <string.h>
+#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 b_mrdivide(const real32_T A[36], const real32_T B[9], real32_T y[36]);
+extern void c_mrdivide(const real32_T A[72], const real32_T B[36], real32_T y[72]);
+extern void mrdivide(const real32_T A[108], const real32_T B[81], real32_T y[108]);
+#endif
+/* End of code generation (mrdivide.h) */
diff --git a/src/modules/attitude_estimator_ekf/codegen/norm.c b/src/modules/attitude_estimator_ekf/codegen/norm.c new file mode 100755 index 000000000..0c418cc7b --- /dev/null +++ b/src/modules/attitude_estimator_ekf/codegen/norm.c @@ -0,0 +1,54 @@ +/*
+ * norm.c
+ *
+ * Code generation for function 'norm'
+ *
+ * C source code generated on: Sat Jan 19 15:25:29 2013
+ *
+ */
+
+/* Include files */
+#include "rt_nonfinite.h"
+#include "attitudeKalmanfilter.h"
+#include "norm.h"
+
+/* Type Definitions */
+
+/* Named Constants */
+
+/* Variable Declarations */
+
+/* Variable Definitions */
+
+/* Function Declarations */
+
+/* Function Definitions */
+
+/*
+ *
+ */
+real32_T norm(const real32_T x[3])
+{
+ real32_T y;
+ real32_T scale;
+ int32_T k;
+ real32_T absxk;
+ real32_T t;
+ y = 0.0F;
+ scale = 1.17549435E-38F;
+ for (k = 0; k < 3; k++) {
+ 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)sqrt(y);
+}
+
+/* End of code generation (norm.c) */
diff --git a/src/modules/attitude_estimator_ekf/codegen/norm.h b/src/modules/attitude_estimator_ekf/codegen/norm.h new file mode 100755 index 000000000..60cf77b57 --- /dev/null +++ b/src/modules/attitude_estimator_ekf/codegen/norm.h @@ -0,0 +1,34 @@ +/*
+ * norm.h
+ *
+ * Code generation for function 'norm'
+ *
+ * C source code generated on: Sat Jan 19 15:25:29 2013
+ *
+ */
+
+#ifndef __NORM_H__
+#define __NORM_H__
+/* Include files */
+#include <math.h>
+#include <stddef.h>
+#include <stdlib.h>
+#include <string.h>
+#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 real32_T norm(const real32_T x[3]);
+#endif
+/* End of code generation (norm.h) */
diff --git a/src/modules/attitude_estimator_ekf/codegen/rdivide.c b/src/modules/attitude_estimator_ekf/codegen/rdivide.c new file mode 100755 index 000000000..d035dae5e --- /dev/null +++ b/src/modules/attitude_estimator_ekf/codegen/rdivide.c @@ -0,0 +1,38 @@ +/*
+ * rdivide.c
+ *
+ * Code generation for function 'rdivide'
+ *
+ * C source code generated on: Sat Jan 19 15:25:29 2013
+ *
+ */
+
+/* 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/src/modules/attitude_estimator_ekf/codegen/rdivide.h b/src/modules/attitude_estimator_ekf/codegen/rdivide.h new file mode 100755 index 000000000..4bbebebe2 --- /dev/null +++ b/src/modules/attitude_estimator_ekf/codegen/rdivide.h @@ -0,0 +1,34 @@ +/*
+ * rdivide.h
+ *
+ * Code generation for function 'rdivide'
+ *
+ * C source code generated on: Sat Jan 19 15:25:29 2013
+ *
+ */
+
+#ifndef __RDIVIDE_H__
+#define __RDIVIDE_H__
+/* Include files */
+#include <math.h>
+#include <stddef.h>
+#include <stdlib.h>
+#include <string.h>
+#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/src/modules/attitude_estimator_ekf/codegen/rtGetInf.c b/src/modules/attitude_estimator_ekf/codegen/rtGetInf.c new file mode 100755 index 000000000..34164d104 --- /dev/null +++ b/src/modules/attitude_estimator_ekf/codegen/rtGetInf.c @@ -0,0 +1,139 @@ +/*
+ * rtGetInf.c
+ *
+ * Code generation for function 'attitudeKalmanfilter'
+ *
+ * C source code generated on: Sat Jan 19 15:25:29 2013
+ *
+ */
+
+/*
+ * Abstract:
+ * MATLAB for code generation function to initialize non-finite, Inf and MinusInf
+ */
+#include "rtGetInf.h"
+#define NumBitsPerChar 8U
+
+/* Function: rtGetInf ==================================================
+ * Abstract:
+ * Initialize rtInf needed by the generated code.
+ * Inf is initialized as non-signaling. Assumes IEEE.
+ */
+real_T rtGetInf(void)
+{
+ size_t bitsPerReal = sizeof(real_T) * (NumBitsPerChar);
+ real_T inf = 0.0;
+ if (bitsPerReal == 32U) {
+ inf = rtGetInfF();
+ } else {
+ uint16_T one = 1U;
+ enum {
+ LittleEndian,
+ BigEndian
+ } machByteOrder = (*((uint8_T *) &one) == 1U) ? LittleEndian : BigEndian;
+ switch (machByteOrder) {
+ case LittleEndian:
+ {
+ union {
+ LittleEndianIEEEDouble bitVal;
+ real_T fltVal;
+ } tmpVal;
+
+ tmpVal.bitVal.words.wordH = 0x7FF00000U;
+ tmpVal.bitVal.words.wordL = 0x00000000U;
+ inf = tmpVal.fltVal;
+ break;
+ }
+
+ case BigEndian:
+ {
+ union {
+ BigEndianIEEEDouble bitVal;
+ real_T fltVal;
+ } tmpVal;
+
+ tmpVal.bitVal.words.wordH = 0x7FF00000U;
+ tmpVal.bitVal.words.wordL = 0x00000000U;
+ inf = tmpVal.fltVal;
+ break;
+ }
+ }
+ }
+
+ return inf;
+}
+
+/* Function: rtGetInfF ==================================================
+ * Abstract:
+ * Initialize rtInfF needed by the generated code.
+ * Inf is initialized as non-signaling. Assumes IEEE.
+ */
+real32_T rtGetInfF(void)
+{
+ IEEESingle infF;
+ infF.wordL.wordLuint = 0x7F800000U;
+ return infF.wordL.wordLreal;
+}
+
+/* Function: rtGetMinusInf ==================================================
+ * Abstract:
+ * Initialize rtMinusInf needed by the generated code.
+ * Inf is initialized as non-signaling. Assumes IEEE.
+ */
+real_T rtGetMinusInf(void)
+{
+ size_t bitsPerReal = sizeof(real_T) * (NumBitsPerChar);
+ real_T minf = 0.0;
+ if (bitsPerReal == 32U) {
+ minf = rtGetMinusInfF();
+ } else {
+ uint16_T one = 1U;
+ enum {
+ LittleEndian,
+ BigEndian
+ } machByteOrder = (*((uint8_T *) &one) == 1U) ? LittleEndian : BigEndian;
+ switch (machByteOrder) {
+ case LittleEndian:
+ {
+ union {
+ LittleEndianIEEEDouble bitVal;
+ real_T fltVal;
+ } tmpVal;
+
+ tmpVal.bitVal.words.wordH = 0xFFF00000U;
+ tmpVal.bitVal.words.wordL = 0x00000000U;
+ minf = tmpVal.fltVal;
+ break;
+ }
+
+ case BigEndian:
+ {
+ union {
+ BigEndianIEEEDouble bitVal;
+ real_T fltVal;
+ } tmpVal;
+
+ tmpVal.bitVal.words.wordH = 0xFFF00000U;
+ tmpVal.bitVal.words.wordL = 0x00000000U;
+ minf = tmpVal.fltVal;
+ break;
+ }
+ }
+ }
+
+ return minf;
+}
+
+/* Function: rtGetMinusInfF ==================================================
+ * Abstract:
+ * Initialize rtMinusInfF needed by the generated code.
+ * Inf is initialized as non-signaling. Assumes IEEE.
+ */
+real32_T rtGetMinusInfF(void)
+{
+ IEEESingle minfF;
+ minfF.wordL.wordLuint = 0xFF800000U;
+ return minfF.wordL.wordLreal;
+}
+
+/* End of code generation (rtGetInf.c) */
diff --git a/src/modules/attitude_estimator_ekf/codegen/rtGetInf.h b/src/modules/attitude_estimator_ekf/codegen/rtGetInf.h new file mode 100755 index 000000000..145373cd0 --- /dev/null +++ b/src/modules/attitude_estimator_ekf/codegen/rtGetInf.h @@ -0,0 +1,23 @@ +/*
+ * rtGetInf.h
+ *
+ * Code generation for function 'attitudeKalmanfilter'
+ *
+ * C source code generated on: Sat Jan 19 15:25:29 2013
+ *
+ */
+
+#ifndef __RTGETINF_H__
+#define __RTGETINF_H__
+
+#include <stddef.h>
+#include "rtwtypes.h"
+#include "rt_nonfinite.h"
+
+extern real_T rtGetInf(void);
+extern real32_T rtGetInfF(void);
+extern real_T rtGetMinusInf(void);
+extern real32_T rtGetMinusInfF(void);
+
+#endif
+/* End of code generation (rtGetInf.h) */
diff --git a/src/modules/attitude_estimator_ekf/codegen/rtGetNaN.c b/src/modules/attitude_estimator_ekf/codegen/rtGetNaN.c new file mode 100755 index 000000000..d84ca9573 --- /dev/null +++ b/src/modules/attitude_estimator_ekf/codegen/rtGetNaN.c @@ -0,0 +1,96 @@ +/*
+ * rtGetNaN.c
+ *
+ * Code generation for function 'attitudeKalmanfilter'
+ *
+ * C source code generated on: Sat Jan 19 15:25:29 2013
+ *
+ */
+
+/*
+ * Abstract:
+ * MATLAB for code generation function to initialize non-finite, NaN
+ */
+#include "rtGetNaN.h"
+#define NumBitsPerChar 8U
+
+/* Function: rtGetNaN ==================================================
+ * Abstract:
+ * Initialize rtNaN needed by the generated code.
+ * NaN is initialized as non-signaling. Assumes IEEE.
+ */
+real_T rtGetNaN(void)
+{
+ size_t bitsPerReal = sizeof(real_T) * (NumBitsPerChar);
+ real_T nan = 0.0;
+ if (bitsPerReal == 32U) {
+ nan = rtGetNaNF();
+ } else {
+ uint16_T one = 1U;
+ enum {
+ LittleEndian,
+ BigEndian
+ } machByteOrder = (*((uint8_T *) &one) == 1U) ? LittleEndian : BigEndian;
+ switch (machByteOrder) {
+ case LittleEndian:
+ {
+ union {
+ LittleEndianIEEEDouble bitVal;
+ real_T fltVal;
+ } tmpVal;
+
+ tmpVal.bitVal.words.wordH = 0xFFF80000U;
+ tmpVal.bitVal.words.wordL = 0x00000000U;
+ nan = tmpVal.fltVal;
+ break;
+ }
+
+ case BigEndian:
+ {
+ union {
+ BigEndianIEEEDouble bitVal;
+ real_T fltVal;
+ } tmpVal;
+
+ tmpVal.bitVal.words.wordH = 0x7FFFFFFFU;
+ tmpVal.bitVal.words.wordL = 0xFFFFFFFFU;
+ nan = tmpVal.fltVal;
+ break;
+ }
+ }
+ }
+
+ return nan;
+}
+
+/* Function: rtGetNaNF ==================================================
+ * Abstract:
+ * Initialize rtNaNF needed by the generated code.
+ * NaN is initialized as non-signaling. Assumes IEEE.
+ */
+real32_T rtGetNaNF(void)
+{
+ IEEESingle nanF = { { 0 } };
+ uint16_T one = 1U;
+ enum {
+ LittleEndian,
+ BigEndian
+ } machByteOrder = (*((uint8_T *) &one) == 1U) ? LittleEndian : BigEndian;
+ switch (machByteOrder) {
+ case LittleEndian:
+ {
+ nanF.wordL.wordLuint = 0xFFC00000U;
+ break;
+ }
+
+ case BigEndian:
+ {
+ nanF.wordL.wordLuint = 0x7FFFFFFFU;
+ break;
+ }
+ }
+
+ return nanF.wordL.wordLreal;
+}
+
+/* End of code generation (rtGetNaN.c) */
diff --git a/src/modules/attitude_estimator_ekf/codegen/rtGetNaN.h b/src/modules/attitude_estimator_ekf/codegen/rtGetNaN.h new file mode 100755 index 000000000..65fdaa96f --- /dev/null +++ b/src/modules/attitude_estimator_ekf/codegen/rtGetNaN.h @@ -0,0 +1,21 @@ +/*
+ * rtGetNaN.h
+ *
+ * Code generation for function 'attitudeKalmanfilter'
+ *
+ * C source code generated on: Sat Jan 19 15:25:29 2013
+ *
+ */
+
+#ifndef __RTGETNAN_H__
+#define __RTGETNAN_H__
+
+#include <stddef.h>
+#include "rtwtypes.h"
+#include "rt_nonfinite.h"
+
+extern real_T rtGetNaN(void);
+extern real32_T rtGetNaNF(void);
+
+#endif
+/* End of code generation (rtGetNaN.h) */
diff --git a/src/modules/attitude_estimator_ekf/codegen/rt_defines.h b/src/modules/attitude_estimator_ekf/codegen/rt_defines.h new file mode 100755 index 000000000..356498363 --- /dev/null +++ b/src/modules/attitude_estimator_ekf/codegen/rt_defines.h @@ -0,0 +1,24 @@ +/*
+ * rt_defines.h
+ *
+ * Code generation for function 'attitudeKalmanfilter'
+ *
+ * C source code generated on: Sat Jan 19 15:25:29 2013
+ *
+ */
+
+#ifndef __RT_DEFINES_H__
+#define __RT_DEFINES_H__
+
+#include <stdlib.h>
+
+#define RT_PI 3.14159265358979323846
+#define RT_PIF 3.1415927F
+#define RT_LN_10 2.30258509299404568402
+#define RT_LN_10F 2.3025851F
+#define RT_LOG10E 0.43429448190325182765
+#define RT_LOG10EF 0.43429449F
+#define RT_E 2.7182818284590452354
+#define RT_EF 2.7182817F
+#endif
+/* End of code generation (rt_defines.h) */
diff --git a/src/modules/attitude_estimator_ekf/codegen/rt_nonfinite.c b/src/modules/attitude_estimator_ekf/codegen/rt_nonfinite.c new file mode 100755 index 000000000..303d1d9d2 --- /dev/null +++ b/src/modules/attitude_estimator_ekf/codegen/rt_nonfinite.c @@ -0,0 +1,87 @@ +/*
+ * rt_nonfinite.c
+ *
+ * Code generation for function 'attitudeKalmanfilter'
+ *
+ * C source code generated on: Sat Jan 19 15:25:29 2013
+ *
+ */
+
+/*
+ * Abstract:
+ * MATLAB for code generation function to initialize non-finites,
+ * (Inf, NaN and -Inf).
+ */
+#include "rt_nonfinite.h"
+#include "rtGetNaN.h"
+#include "rtGetInf.h"
+
+real_T rtInf;
+real_T rtMinusInf;
+real_T rtNaN;
+real32_T rtInfF;
+real32_T rtMinusInfF;
+real32_T rtNaNF;
+
+/* Function: rt_InitInfAndNaN ==================================================
+ * Abstract:
+ * Initialize the rtInf, rtMinusInf, and rtNaN needed by the
+ * generated code. NaN is initialized as non-signaling. Assumes IEEE.
+ */
+void rt_InitInfAndNaN(size_t realSize)
+{
+ (void) (realSize);
+ rtNaN = rtGetNaN();
+ rtNaNF = rtGetNaNF();
+ rtInf = rtGetInf();
+ rtInfF = rtGetInfF();
+ rtMinusInf = rtGetMinusInf();
+ rtMinusInfF = rtGetMinusInfF();
+}
+
+/* Function: rtIsInf ==================================================
+ * Abstract:
+ * Test if value is infinite
+ */
+boolean_T rtIsInf(real_T value)
+{
+ return ((value==rtInf || value==rtMinusInf) ? 1U : 0U);
+}
+
+/* Function: rtIsInfF =================================================
+ * Abstract:
+ * Test if single-precision value is infinite
+ */
+boolean_T rtIsInfF(real32_T value)
+{
+ return(((value)==rtInfF || (value)==rtMinusInfF) ? 1U : 0U);
+}
+
+/* Function: rtIsNaN ==================================================
+ * Abstract:
+ * Test if value is not a number
+ */
+boolean_T rtIsNaN(real_T value)
+{
+#if defined(_MSC_VER) && (_MSC_VER <= 1200)
+ return _isnan(value)? TRUE:FALSE;
+#else
+ return (value!=value)? 1U:0U;
+#endif
+}
+
+/* Function: rtIsNaNF =================================================
+ * Abstract:
+ * Test if single-precision value is not a number
+ */
+boolean_T rtIsNaNF(real32_T value)
+{
+#if defined(_MSC_VER) && (_MSC_VER <= 1200)
+ return _isnan((real_T)value)? true:false;
+#else
+ return (value!=value)? 1U:0U;
+#endif
+}
+
+
+/* End of code generation (rt_nonfinite.c) */
diff --git a/src/modules/attitude_estimator_ekf/codegen/rt_nonfinite.h b/src/modules/attitude_estimator_ekf/codegen/rt_nonfinite.h new file mode 100755 index 000000000..bd56b30d9 --- /dev/null +++ b/src/modules/attitude_estimator_ekf/codegen/rt_nonfinite.h @@ -0,0 +1,53 @@ +/*
+ * rt_nonfinite.h
+ *
+ * Code generation for function 'attitudeKalmanfilter'
+ *
+ * C source code generated on: Sat Jan 19 15:25:29 2013
+ *
+ */
+
+#ifndef __RT_NONFINITE_H__
+#define __RT_NONFINITE_H__
+
+#if defined(_MSC_VER) && (_MSC_VER <= 1200)
+#include <float.h>
+#endif
+#include <stddef.h>
+#include "rtwtypes.h"
+
+extern real_T rtInf;
+extern real_T rtMinusInf;
+extern real_T rtNaN;
+extern real32_T rtInfF;
+extern real32_T rtMinusInfF;
+extern real32_T rtNaNF;
+extern void rt_InitInfAndNaN(size_t realSize);
+extern boolean_T rtIsInf(real_T value);
+extern boolean_T rtIsInfF(real32_T value);
+extern boolean_T rtIsNaN(real_T value);
+extern boolean_T rtIsNaNF(real32_T value);
+
+typedef struct {
+ struct {
+ uint32_T wordH;
+ uint32_T wordL;
+ } words;
+} BigEndianIEEEDouble;
+
+typedef struct {
+ struct {
+ uint32_T wordL;
+ uint32_T wordH;
+ } words;
+} LittleEndianIEEEDouble;
+
+typedef struct {
+ union {
+ real32_T wordLreal;
+ uint32_T wordLuint;
+ } wordL;
+} IEEESingle;
+
+#endif
+/* End of code generation (rt_nonfinite.h) */
diff --git a/src/modules/attitude_estimator_ekf/codegen/rtwtypes.h b/src/modules/attitude_estimator_ekf/codegen/rtwtypes.h new file mode 100755 index 000000000..9a5c96267 --- /dev/null +++ b/src/modules/attitude_estimator_ekf/codegen/rtwtypes.h @@ -0,0 +1,159 @@ +/*
+ * rtwtypes.h
+ *
+ * Code generation for function 'attitudeKalmanfilter'
+ *
+ * C source code generated on: Sat Jan 19 15:25:29 2013
+ *
+ */
+
+#ifndef __RTWTYPES_H__
+#define __RTWTYPES_H__
+#ifndef TRUE
+# define TRUE (1U)
+#endif
+#ifndef FALSE
+# define FALSE (0U)
+#endif
+#ifndef __TMWTYPES__
+#define __TMWTYPES__
+
+#include <limits.h>
+
+/*=======================================================================*
+ * Target hardware information
+ * Device type: Generic->MATLAB Host Computer
+ * 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
+ *=======================================================================*/
+
+/*=======================================================================*
+ * Fixed width word size data types: *
+ * int8_T, int16_T, int32_T - signed 8, 16, or 32 bit integers *
+ * uint8_T, uint16_T, uint32_T - unsigned 8, 16, or 32 bit integers *
+ * real32_T, real64_T - 32 and 64 bit floating point numbers *
+ *=======================================================================*/
+
+typedef signed char int8_T;
+typedef unsigned char uint8_T;
+typedef short int16_T;
+typedef unsigned short uint16_T;
+typedef int int32_T;
+typedef unsigned int uint32_T;
+typedef float real32_T;
+typedef double real64_T;
+
+/*===========================================================================*
+ * Generic type definitions: real_T, time_T, boolean_T, int_T, uint_T, *
+ * ulong_T, char_T and byte_T. *
+ *===========================================================================*/
+
+typedef double real_T;
+typedef double time_T;
+typedef unsigned char boolean_T;
+typedef int int_T;
+typedef unsigned int uint_T;
+typedef unsigned long ulong_T;
+typedef char char_T;
+typedef char_T byte_T;
+
+/*===========================================================================*
+ * Complex number type definitions *
+ *===========================================================================*/
+#define CREAL_T
+ typedef struct {
+ real32_T re;
+ real32_T im;
+ } creal32_T;
+
+ typedef struct {
+ real64_T re;
+ real64_T im;
+ } creal64_T;
+
+ typedef struct {
+ real_T re;
+ real_T im;
+ } creal_T;
+
+ typedef struct {
+ int8_T re;
+ int8_T im;
+ } cint8_T;
+
+ typedef struct {
+ uint8_T re;
+ uint8_T im;
+ } cuint8_T;
+
+ typedef struct {
+ int16_T re;
+ int16_T im;
+ } cint16_T;
+
+ typedef struct {
+ uint16_T re;
+ uint16_T im;
+ } cuint16_T;
+
+ typedef struct {
+ int32_T re;
+ int32_T im;
+ } cint32_T;
+
+ typedef struct {
+ uint32_T re;
+ uint32_T im;
+ } cuint32_T;
+
+
+/*=======================================================================*
+ * Min and Max: *
+ * int8_T, int16_T, int32_T - signed 8, 16, or 32 bit integers *
+ * uint8_T, uint16_T, uint32_T - unsigned 8, 16, or 32 bit integers *
+ *=======================================================================*/
+
+#define MAX_int8_T ((int8_T)(127))
+#define MIN_int8_T ((int8_T)(-128))
+#define MAX_uint8_T ((uint8_T)(255))
+#define MIN_uint8_T ((uint8_T)(0))
+#define MAX_int16_T ((int16_T)(32767))
+#define MIN_int16_T ((int16_T)(-32768))
+#define MAX_uint16_T ((uint16_T)(65535))
+#define MIN_uint16_T ((uint16_T)(0))
+#define MAX_int32_T ((int32_T)(2147483647))
+#define MIN_int32_T ((int32_T)(-2147483647-1))
+#define MAX_uint32_T ((uint32_T)(0xFFFFFFFFU))
+#define MIN_uint32_T ((uint32_T)(0))
+
+/* Logical type definitions */
+#if !defined(__cplusplus) && !defined(__true_false_are_keywords)
+# ifndef false
+# define false (0U)
+# endif
+# ifndef true
+# define true (1U)
+# endif
+#endif
+
+/*
+ * MATLAB for code generation assumes the code is compiled on a target using a 2's compliment representation
+ * for signed integer values.
+ */
+#if ((SCHAR_MIN + 1) != -SCHAR_MAX)
+#error "This code must be compiled using a 2's complement representation for signed integer values"
+#endif
+
+/*
+ * Maximum length of a MATLAB identifier (function/variable)
+ * including the null-termination character. Referenced by
+ * rt_logging.c and rt_matrx.c.
+ */
+#define TMW_NAME_LENGTH_MAX 64
+
+#endif
+#endif
+/* End of code generation (rtwtypes.h) */
diff --git a/src/modules/attitude_estimator_ekf/module.mk b/src/modules/attitude_estimator_ekf/module.mk new file mode 100644 index 000000000..d98647f99 --- /dev/null +++ b/src/modules/attitude_estimator_ekf/module.mk @@ -0,0 +1,52 @@ +############################################################################ +# +# Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +# +# Attitude estimator (Extended Kalman Filter) +# + +MODULE_COMMAND = attitude_estimator_ekf + +SRCS = attitude_estimator_ekf_main.cpp \ + attitude_estimator_ekf_params.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 \ + codegen/rtGetInf.c \ + codegen/rtGetNaN.c \ + codegen/norm.c \ + codegen/cross.c |