fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-23 06:33:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

Merged new EKF version

Browse Source
This commit is contained in:
Lorenz Meier
2012-10-16 11:10:09 +02:00
parent a720bfff5e
commit fef4362e79
41 changed files with 709 additions and 722 deletions
Download Patch File Download Diff File Expand all files Collapse all files
apps/attitude_estimator_ekf/.context
View File
apps/attitude_estimator_ekf/Makefile
Regular → Executable
+3 -2
View File
@@ -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
apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c
Regular → Executable
+203 -183
View File
@@ -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};
if (!initialized) {
/* 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;
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);
}
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];
// 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);
}
}
apps/attitude_estimator_ekf/attitude_estimator_ekf_params.c
Regular → Executable
+6 -6
View File
@@ -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);
apps/attitude_estimator_ekf/attitude_estimator_ekf_params.h
Regular → Executable
View File
apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter.c
+254 -213
View File
@@ -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:167' m_n_b = x_aposteriori(10:12)./norm(x_aposteriori(10:12)); */
b_y = norm(*(real32_T (*)[3])&x_aposteriori[9]);
/* 'attitudeKalmanfilter:169' y_n_b=cross(z_n_b,m_n_b); */
/* 'attitudeKalmanfilter:176' z_n_b = -x_aposteriori(7:9)./norm(x_aposteriori(7:9)); */
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] = -x_aposteriori[i + 6];
}
cross(z_n_b, x_n_b, y_n_b);
rdivide(x_n_b, norm(*(real32_T (*)[3])&x_aposteriori[6]), z_n_b);
/* 'attitudeKalmanfilter:170' y_n_b=y_n_b./norm(y_n_b); */
y = norm(y_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++) {
y_n_b[i] /= y;
x_n_b[i] = m_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);
cross(z_n_b, x_n_b, m_n_b);
/* 'attitudeKalmanfilter:173' x_n_b=x_n_b./norm(x_n_b); */
y = norm(x_n_b);
/* 'attitudeKalmanfilter:180' y_n_b=y_n_b./norm(y_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];
x_n_b[i] = m_n_b[i];
}
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:183' 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:189' 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] = 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) */
apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter.h
+2 -2
View File
@@ -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"
apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.c
+1 -1
View File
@@ -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
*
*/
apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.h
+2 -2
View File
@@ -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"
apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.c
+1 -1
View File
@@ -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
*
*/
apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.h
+2 -2
View File
@@ -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"
apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_types.h
+1 -1
View File
@@ -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
*
*/
apps/attitude_estimator_ekf/codegen/cross.c
+1 -1
View File
@@ -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
*
*/
apps/attitude_estimator_ekf/codegen/cross.h
+2 -2
View File
@@ -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"
apps/attitude_estimator_ekf/codegen/diag.c
+8 -20
View File
@@ -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];
}
}
apps/attitude_estimator_ekf/codegen/diag.h
+5 -6
View File
@@ -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) */
apps/attitude_estimator_ekf/codegen/eye.c
+3 -3
View File
@@ -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;
}
apps/attitude_estimator_ekf/codegen/eye.h
+2 -2
View File
@@ -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"
apps/attitude_estimator_ekf/codegen/mrdivide.c
+104 -114
View File
@@ -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];
}
}
}
apps/attitude_estimator_ekf/codegen/mrdivide.h
+2 -2
View File
@@ -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"
apps/attitude_estimator_ekf/codegen/norm.c
+11 -19
View File
@@ -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) */
apps/attitude_estimator_ekf/codegen/norm.h
+2 -2
View File
@@ -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"
apps/attitude_estimator_ekf/codegen/power.c
-84
View File
@@ -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) */
apps/attitude_estimator_ekf/codegen/rdivide.c
Executable
+38
View File
@@ -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) */
apps/attitude_estimator_ekf/codegen/power.h → apps/attitude_estimator_ekf/codegen/rdivide.h
+8 -8
View File
@@ -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) */
apps/attitude_estimator_ekf/codegen/rtGetInf.c
+1 -1
View File
@@ -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
*
*/
apps/attitude_estimator_ekf/codegen/rtGetInf.h
+1 -1
View File
@@ -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
*
*/
apps/attitude_estimator_ekf/codegen/rtGetNaN.c
+1 -1
View File
@@ -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
*
*/
apps/attitude_estimator_ekf/codegen/rtGetNaN.h
+1 -1
View File
@@ -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
*
*/
apps/attitude_estimator_ekf/codegen/rt_defines.h
+1 -1
View File
@@ -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
*
*/
apps/attitude_estimator_ekf/codegen/rt_nonfinite.c
+1 -1
View File
@@ -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
*
*/
apps/attitude_estimator_ekf/codegen/rt_nonfinite.h
+1 -1
View File
@@ -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
*
*/
apps/attitude_estimator_ekf/codegen/rtwtypes.h
+3 -3
View File
@@ -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
*=======================================================================*/
/*=======================================================================*
apps/multirotor_att_control/Makefile
Regular → Executable
View File
apps/multirotor_att_control/multirotor_att_control_main.c
Regular → Executable
+4 -2
View File
@@ -172,7 +172,7 @@ mc_thread_main(int argc, char *argv[])
rates_sp.pitch = offboard_sp.p2;
rates_sp.yaw = offboard_sp.p3;
rates_sp.thrust = offboard_sp.p4;
printf("thrust_rate=%8.4f\n",offboard_sp.p4);
//printf("thrust_rate=%8.4f\n",offboard_sp.p4);
rates_sp.timestamp = hrt_absolute_time();
orb_publish(ORB_ID(vehicle_rates_setpoint), rates_sp_pub, &rates_sp);
} else if (offboard_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE) {
@@ -180,7 +180,7 @@ mc_thread_main(int argc, char *argv[])
att_sp.pitch_body = offboard_sp.p2;
att_sp.yaw_body = offboard_sp.p3;
att_sp.thrust = offboard_sp.p4;
printf("thrust_att=%8.4f\n",offboard_sp.p4);
//printf("thrust_att=%8.4f\n",offboard_sp.p4);
att_sp.timestamp = hrt_absolute_time();
/* STEP 2: publish the result to the vehicle actuators */
orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp);
@@ -196,6 +196,7 @@ mc_thread_main(int argc, char *argv[])
rates_sp.pitch = manual.pitch;
rates_sp.yaw = manual.yaw;
rates_sp.thrust = manual.throttle;
//printf("rates\n");
rates_sp.timestamp = hrt_absolute_time();
}
@@ -212,6 +213,7 @@ mc_thread_main(int argc, char *argv[])
orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp);
if (motor_test_mode) {
printf("testmode");
att_sp.roll_body = 0.0f;
att_sp.pitch_body = 0.0f;
att_sp.yaw_body = 0.0f;
apps/multirotor_att_control/multirotor_attitude_control.c
Regular → Executable
+3 -3
View File
@@ -238,12 +238,12 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s
/* control pitch (forward) output */
float pitch_control = pid_calculate(&pitch_controller, att_sp->pitch_body + p.att_xoff,
att->pitch, att->pitchspeed, deltaT);
att->pitch, att->pitchspeed, deltaT)*1/10.0f;
/* control roll (left/right) output */
float roll_control = pid_calculate(&roll_controller, att_sp->roll_body + p.att_yoff,
att->roll, att->rollspeed, deltaT);
att->roll, att->rollspeed, deltaT)*1/10.0f;
/* control yaw rate */
float yaw_rate_control = pid_calculate(&yaw_speed_controller, att_sp->yaw_body, att->yawspeed, 0.0f, deltaT);
float yaw_rate_control = pid_calculate(&yaw_speed_controller, att_sp->yaw_body, att->yawspeed, 0.0f, deltaT)*1/10.0f;
/*
* compensate the vertical loss of thrust
apps/multirotor_att_control/multirotor_attitude_control.h
Regular → Executable
View File
apps/multirotor_att_control/multirotor_rate_control.c
Regular → Executable
+12 -12
View File
@@ -50,16 +50,16 @@
#include <systemlib/param/param.h>
#include <arch/board/up_hrt.h>
// PARAM_DEFINE_FLOAT(MC_YAWRATE_P, 0.08f); /* same on Flamewheel */
// PARAM_DEFINE_FLOAT(MC_YAWRATE_I, 0.02f);
// PARAM_DEFINE_FLOAT(MC_YAWRATE_AWU, 0.02f);
// PARAM_DEFINE_FLOAT(MC_YAWRATE_LIM, 0.1f);
PARAM_DEFINE_FLOAT(MC_YAWRATE_P, 20.0f); /* same on Flamewheel */
PARAM_DEFINE_FLOAT(MC_YAWRATE_I, 0.0f);
PARAM_DEFINE_FLOAT(MC_YAWRATE_AWU, 0.0f);
PARAM_DEFINE_FLOAT(MC_YAWRATE_LIM, 0.f);
PARAM_DEFINE_FLOAT(MC_ATTRATE_P, 0.2f); /* 0.15 F405 Flamewheel */
PARAM_DEFINE_FLOAT(MC_ATTRATE_D, 0.0f);
//PARAM_DEFINE_FLOAT(MC_ATTRATE_I, 0.0f);
//PARAM_DEFINE_FLOAT(MC_ATTRATE_AWU, 0.05f);
PARAM_DEFINE_FLOAT(MC_ATTRATE_LIM, 8.0f); /**< roughly < 500 deg/s limit */
PARAM_DEFINE_FLOAT(MC_ATTRATE_P, 40.0f); /* 0.15 F405 Flamewheel */
PARAM_DEFINE_FLOAT(MC_ATTRATE_D, 0.05f);
PARAM_DEFINE_FLOAT(MC_ATTRATE_I, 0.0f);
PARAM_DEFINE_FLOAT(MC_ATTRATE_AWU, 0.05f);
PARAM_DEFINE_FLOAT(MC_ATTRATE_LIM, 10.0f); /**< roughly < 500 deg/s limit */
struct mc_rate_control_params {
@@ -175,14 +175,14 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp,
/* control pitch (forward) output */
float pitch_control = p.attrate_p * deltaT *(rate_sp->pitch-rates[1])-p.attrate_d*(pitch_control_last);
float pitch_control = p.attrate_p * deltaT *((rate_sp->pitch)*p.attrate_lim-rates[1])-p.attrate_d*(pitch_control_last);
pitch_control_last=pitch_control;
/* control roll (left/right) output */
float roll_control = p.attrate_p * deltaT * (rate_sp->roll-rates[0])-p.attrate_d*(roll_control_last);
float roll_control = p.attrate_p * deltaT * ((rate_sp->roll)*p.attrate_lim-rates[0])-p.attrate_d*(roll_control_last);
roll_control_last=roll_control;
/* control yaw rate */
float yaw_rate_control = p.yawrate_p * deltaT * (rate_sp->yaw-rates[2] );
float yaw_rate_control = p.yawrate_p * deltaT * ((rate_sp->yaw)*p.attrate_lim-rates[2] );
actuators->control[0] = roll_control;
actuators->control[1] = pitch_control;
apps/multirotor_att_control/multirotor_rate_control.h
Regular → Executable
View File
apps/uORB/topics/sensor_combined.h
+18 -18
View File
@@ -77,33 +77,33 @@ struct sensor_combined_s {
/* NOTE: Ordering of fields optimized to align to 32 bit / 4 bytes Change with consideration only */
uint64_t timestamp; /**< Timestamp in microseconds since boot LOGME */
uint64_t timestamp; /**< Timestamp in microseconds since boot */
int16_t gyro_raw[3]; /**< Raw sensor values of angular velocity LOGME */
uint16_t gyro_counter; /**< Number of raw measurments taken LOGME */
float gyro_rad_s[3]; /**< Angular velocity in radian per seconds LOGME */
int16_t gyro_raw[3]; /**< Raw sensor values of angular velocity */
uint16_t gyro_counter; /**< Number of raw measurments taken */
float gyro_rad_s[3]; /**< Angular velocity in radian per seconds */
int16_t accelerometer_raw[3]; /**< Raw acceleration in NED body frame LOGME */
uint32_t accelerometer_counter; /**< Number of raw acc measurements taken LOGME */
float accelerometer_m_s2[3]; /**< Acceleration in NED body frame, in m/s^2 LOGME */
int16_t accelerometer_raw[3]; /**< Raw acceleration in NED body frame */
uint32_t accelerometer_counter; /**< Number of raw acc measurements taken */
float accelerometer_m_s2[3]; /**< Acceleration in NED body frame, in m/s^2 */
int accelerometer_mode; /**< Accelerometer measurement mode */
float accelerometer_range_m_s2; /**< Accelerometer measurement range in m/s^2 */
int16_t magnetometer_raw[3]; /**< Raw magnetic field in NED body frame LOGME */
float magnetometer_ga[3]; /**< Magnetic field in NED body frame, in Gauss LOGME */
int16_t magnetometer_raw[3]; /**< Raw magnetic field in NED body frame */
float magnetometer_ga[3]; /**< Magnetic field in NED body frame, in Gauss */
int magnetometer_mode; /**< Magnetometer measurement mode */
float magnetometer_range_ga; /**< ± measurement range in Gauss */
float magnetometer_cuttoff_freq_hz; /**< Internal analog low pass frequency of sensor */
uint32_t magnetometer_counter; /**< Number of raw mag measurements taken LOGME */
uint32_t magnetometer_counter; /**< Number of raw mag measurements taken */
float baro_pres_mbar; /**< Barometric pressure, already temp. comp. LOGME */
float baro_alt_meter; /**< Altitude, already temp. comp. LOGME */
float baro_temp_celcius; /**< Temperature in degrees celsius LOGME */
float battery_voltage_v; /**< Battery voltage in volts, filtered LOGME */
float adc_voltage_v[3]; /**< ADC voltages of ADC Chan 11/12/13 or -1 LOGME */
uint32_t baro_counter; /**< Number of raw baro measurements taken LOGME */
uint32_t battery_voltage_counter; /**< Number of voltage measurements taken LOGME */
bool battery_voltage_valid; /**< True if battery voltage can be measured LOGME */
float baro_pres_mbar; /**< Barometric pressure, already temp. comp. */
float baro_alt_meter; /**< Altitude, already temp. comp. */
float baro_temp_celcius; /**< Temperature in degrees celsius */
float battery_voltage_v; /**< Battery voltage in volts, filtered */
float adc_voltage_v[3]; /**< ADC voltages of ADC Chan 11/12/13 or -1 */
uint32_t baro_counter; /**< Number of raw baro measurements taken */
uint32_t battery_voltage_counter; /**< Number of voltage measurements taken */
bool battery_voltage_valid; /**< True if battery voltage can be measured */
};