From 053ce0e2f8c445dae046658ba5741adbd79d6ddb Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 3 Oct 2012 14:45:55 +0200 Subject: [PATCH] Exposed measurement noise covariance and process noise covariance as MAVLink parameters for attitude EKF --- apps/attitude_estimator_ekf/Makefile | 1 + .../attitude_estimator_ekf_main.c | 370 +++++++++--------- .../attitude_estimator_ekf_params.c | 131 +++++++ .../attitude_estimator_ekf_params.h | 64 +++ 4 files changed, 379 insertions(+), 187 deletions(-) create mode 100644 apps/attitude_estimator_ekf/attitude_estimator_ekf_params.c create mode 100644 apps/attitude_estimator_ekf/attitude_estimator_ekf_params.h diff --git a/apps/attitude_estimator_ekf/Makefile b/apps/attitude_estimator_ekf/Makefile index a8f80fd4c7..e5620138a5 100644 --- a/apps/attitude_estimator_ekf/Makefile +++ b/apps/attitude_estimator_ekf/Makefile @@ -36,6 +36,7 @@ PRIORITY = SCHED_PRIORITY_DEFAULT STACKSIZE = 2048 CSRCS = attitude_estimator_ekf_main.c \ + attitude_estimator_ekf_params.c \ codegen/eye.c \ codegen/attitudeKalmanfilter.c \ codegen/mrdivide.c \ diff --git a/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c b/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c index 10405758f1..41f469ae4e 100644 --- a/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c +++ b/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c @@ -58,25 +58,20 @@ #include #include #include +#include #include #include #include "codegen/attitudeKalmanfilter_initialize.h" #include "codegen/attitudeKalmanfilter.h" +#include "attitude_estimator_ekf_params.h" __EXPORT int attitude_estimator_ekf_main(int argc, char *argv[]); - -// #define N_STATES 6 - -// #define PROJECTION_INITIALIZE_COUNTER_LIMIT 5000 -// #define REPROJECTION_COUNTER_LIMIT 125 - static unsigned int loop_interval_alarm = 6500; // loop interval in microseconds static float dt = 1.0f; -/* 0, 0, -9.81, 1, 1, 1, wo (gyro offset), w */ /* state vector x has the following entries [ax,ay,az||mx,my,mz||wox,woy,woz||wx,wy,wz]' */ static float z_k[9]; /**< Measurement vector */ static float x_aposteriori_k[12]; /**< */ @@ -94,6 +89,7 @@ static float P_aposteriori_k[144] = {100.f, 0, 0, 0, 0, 0, 0, 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, @@ -107,11 +103,14 @@ static float P_aposteriori[144] = {100.f, 0, 0, 0, 0, 0, 0, 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 */ -// static float knownConst[15] = {1, 1, 1, 1, 1, 0.04, 4, 0.1, 70, 70, -2000, 9.81, 1, 4, 1}; /**< knownConst has the following entries [PrvaA,PrvarM,PrvarWO,PrvarW||MsvarA,MsvarM,MsvarW] */ + +/* 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 */ @@ -223,6 +222,9 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[]) /* 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)); + /* advertise attitude */ orb_advert_t pub_att = orb_advertise(ORB_ID(vehicle_attitude), &att); @@ -241,18 +243,19 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[]) uint64_t sensor_last_timestamp[3] = {0, 0, 0}; float sensor_update_hz[3] = {0.0f, 0.0f, 0.0f}; - /* process noise covariance */ - float q[12]; - /* measurement noise covariance */ - float r[9]; - /* output euler angles */ - float euler[3] = {0.0f, 0.0f, 0.0f}; + struct attitude_estimator_ekf_params ekf_params; + + struct attitude_estimator_ekf_param_handles ekf_param_handles; + + /* initialize parameter handles */ + parameters_init(&ekf_param_handles); /* Main loop*/ while (!thread_should_exit) { - struct pollfd fds[1] = { + struct pollfd fds[2] = { { .fd = sub_raw, .events = POLLIN }, + { .fd = sub_params, .events = POLLIN } }; int ret = poll(fds, 1, 1000); @@ -262,180 +265,173 @@ 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 */ + struct parameter_update_s update; + orb_copy(ORB_ID(parameter_update), sub_params, &update); - orb_copy(ORB_ID(sensor_combined), sub_raw, &raw); - - /* Calculate data time difference in seconds */ - dt = (raw.timestamp - last_measurement) / 1000000.0f; - last_measurement = raw.timestamp; - uint8_t update_vect[3] = {0, 0, 0}; - - /* Fill in gyro measurements */ - if (sensor_last_count[0] != raw.gyro_counter) { - update_vect[0] = 1; - sensor_last_count[0] = raw.gyro_counter; - sensor_update_hz[0] = 1e6f / (raw.timestamp - sensor_last_timestamp[0]); - sensor_last_timestamp[0] = raw.timestamp; + /* update parameters */ + parameters_update(&ekf_param_handles, &ekf_params); } - z_k[0] = raw.gyro_rad_s[0]; - z_k[1] = raw.gyro_rad_s[1]; - z_k[2] = raw.gyro_rad_s[2]; + /* only run filter if sensor values changed */ + if (fds[0].revents & POLLIN) { - /* 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; + /* get latest measurements */ + orb_copy(ORB_ID(sensor_combined), sub_raw, &raw); + + /* Calculate data time difference in seconds */ + dt = (raw.timestamp - last_measurement) / 1000000.0f; + last_measurement = raw.timestamp; + uint8_t update_vect[3] = {0, 0, 0}; + + /* Fill in gyro measurements */ + if (sensor_last_count[0] != raw.gyro_counter) { + update_vect[0] = 1; + sensor_last_count[0] = raw.gyro_counter; + sensor_update_hz[0] = 1e6f / (raw.timestamp - sensor_last_timestamp[0]); + sensor_last_timestamp[0] = raw.timestamp; + } + + z_k[0] = raw.gyro_rad_s[0]; + z_k[1] = raw.gyro_rad_s[1]; + z_k[2] = raw.gyro_rad_s[2]; + + /* update accelerometer measurements */ + if (sensor_last_count[1] != raw.accelerometer_counter) { + update_vect[1] = 1; + sensor_last_count[1] = raw.accelerometer_counter; + sensor_update_hz[1] = 1e6f / (raw.timestamp - sensor_last_timestamp[1]); + sensor_last_timestamp[1] = raw.timestamp; + } + z_k[3] = raw.accelerometer_m_s2[0]; + z_k[4] = raw.accelerometer_m_s2[1]; + z_k[5] = raw.accelerometer_m_s2[2]; + + /* update magnetometer measurements */ + if (sensor_last_count[2] != raw.magnetometer_counter) { + update_vect[2] = 1; + sensor_last_count[2] = raw.magnetometer_counter; + sensor_update_hz[2] = 1e6f / (raw.timestamp - sensor_last_timestamp[2]); + sensor_last_timestamp[2] = raw.timestamp; + } + z_k[6] = raw.magnetometer_ga[0]; + z_k[7] = raw.magnetometer_ga[1]; + z_k[8] = raw.magnetometer_ga[2]; + + uint64_t now = hrt_absolute_time(); + unsigned int time_elapsed = now - last_run; + last_run = now; + + if (time_elapsed > loop_interval_alarm) { + //TODO: add warning, cpu overload here + // if (overloadcounter == 20) { + // printf("CPU OVERLOAD DETECTED IN ATTITUDE ESTIMATOR EKF (%lu > %lu)\n", time_elapsed, loop_interval_alarm); + // overloadcounter = 0; + // } + + overloadcounter++; + } + + int32_t z_k_sizes = 9; + // float u[4] = {0.0f, 0.0f, 0.0f, 0.0f}; + + static bool const_initialized = false; + + /* initialize with good values once we have a reasonable dt estimate */ + if (!const_initialized /*&& dt < 0.05 && dt > 0.005*/) + { + dt = 0.005f; + parameters_update(&ekf_param_handles, &ekf_params); + + x_aposteriori_k[0] = z_k[0]; + x_aposteriori_k[1] = z_k[1]; + x_aposteriori_k[2] = z_k[2]; + x_aposteriori_k[3] = 0.0f; + x_aposteriori_k[4] = 0.0f; + x_aposteriori_k[5] = 0.0f; + x_aposteriori_k[6] = z_k[3]; + x_aposteriori_k[7] = z_k[4]; + x_aposteriori_k[8] = z_k[5]; + x_aposteriori_k[9] = z_k[6]; + x_aposteriori_k[10] = z_k[7]; + x_aposteriori_k[11] = z_k[8]; + + const_initialized = true; + } + + /* do not execute the filter if not initialized */ + if (!const_initialized) { + continue; + } + + 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]; + + /* 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[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; - q[0] = 1e1f; - q[1] = 1e1f; - q[2] = 1e1f; - /* process noise gyro offset covariance */ - q[3] = 1e-4f; - q[4] = 1e-4f; - q[5] = 1e-4f; - q[6] = 1e-1f; - q[7] = 1e-1f; - q[8] = 1e-1f; - q[9] = 1e-1f; - q[10] = 1e-1f; - q[11] = 1e-1f; - - r[0]= 1e-2f; - r[1]= 1e-2f; - r[2]= 1e-2f; - r[3]= 1e-1f; - r[4]= 1e-1f; - r[5]= 1e-1f; - r[6]= 1e-1f; - r[7]= 1e-1f; - r[8]= 1e-1f; - - 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, q, 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]; - - /* 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); } loopcounter++; diff --git a/apps/attitude_estimator_ekf/attitude_estimator_ekf_params.c b/apps/attitude_estimator_ekf/attitude_estimator_ekf_params.c new file mode 100644 index 0000000000..d95c56368b --- /dev/null +++ b/apps/attitude_estimator_ekf/attitude_estimator_ekf_params.c @@ -0,0 +1,131 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Author: Tobias Naegeli + * Lorenz Meier + * + * 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_Q0, 1e1f); +PARAM_DEFINE_FLOAT(EKF_ATT_Q1, 1e1f); +PARAM_DEFINE_FLOAT(EKF_ATT_Q2, 1e1f); +/* gyro offsets process noise */ +PARAM_DEFINE_FLOAT(EKF_ATT_Q3, 1e-4f); +PARAM_DEFINE_FLOAT(EKF_ATT_Q4, 1e-4f); +PARAM_DEFINE_FLOAT(EKF_ATT_Q5, 1e-4f); +/* accelerometer process noise */ +PARAM_DEFINE_FLOAT(EKF_ATT_Q6, 1e-1f); +PARAM_DEFINE_FLOAT(EKF_ATT_Q7, 1e-1f); +PARAM_DEFINE_FLOAT(EKF_ATT_Q8, 1e-1f); +/* magnetometer process noise */ +PARAM_DEFINE_FLOAT(EKF_ATT_Q9, 1e-1f); +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); +/* 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); +/* magnetometer measurement noise */ +PARAM_DEFINE_FLOAT(EKF_ATT_R6, 1e-1f); +PARAM_DEFINE_FLOAT(EKF_ATT_R7, 1e-1f); +PARAM_DEFINE_FLOAT(EKF_ATT_R8, 1e-1f); + +int parameters_init(struct attitude_estimator_ekf_param_handles *h) +{ + /* PID parameters */ + h->q0 = param_find("EKF_ATT_Q0"); + h->q1 = param_find("EKF_ATT_Q1"); + h->q2 = param_find("EKF_ATT_Q2"); + h->q3 = param_find("EKF_ATT_Q3"); + h->q4 = param_find("EKF_ATT_Q4"); + h->q5 = param_find("EKF_ATT_Q5"); + h->q6 = param_find("EKF_ATT_Q6"); + h->q7 = param_find("EKF_ATT_Q7"); + h->q8 = param_find("EKF_ATT_Q8"); + h->q9 = param_find("EKF_ATT_Q9"); + h->q10 = param_find("EKF_ATT_Q10"); + h->q11 = param_find("EKF_ATT_Q11"); + + h->r0 = param_find("EKF_ATT_R0"); + h->r1 = param_find("EKF_ATT_R1"); + h->r2 = param_find("EKF_ATT_R2"); + h->r3 = param_find("EKF_ATT_R3"); + h->r4 = param_find("EKF_ATT_R4"); + h->r5 = param_find("EKF_ATT_R5"); + h->r6 = param_find("EKF_ATT_R6"); + h->r7 = param_find("EKF_ATT_R7"); + h->r8 = param_find("EKF_ATT_R8"); + + 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->q5, &(p->q[5])); + param_get(h->q6, &(p->q[6])); + param_get(h->q7, &(p->q[7])); + param_get(h->q8, &(p->q[8])); + param_get(h->q9, &(p->q[9])); + param_get(h->q10, &(p->q[10])); + param_get(h->q11, &(p->q[11])); + + 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->r4, &(p->r[4])); + param_get(h->r5, &(p->r[5])); + param_get(h->r6, &(p->r[6])); + param_get(h->r7, &(p->r[7])); + param_get(h->r8, &(p->r[8])); + + return OK; +} diff --git a/apps/attitude_estimator_ekf/attitude_estimator_ekf_params.h b/apps/attitude_estimator_ekf/attitude_estimator_ekf_params.h new file mode 100644 index 0000000000..6a63f97674 --- /dev/null +++ b/apps/attitude_estimator_ekf/attitude_estimator_ekf_params.h @@ -0,0 +1,64 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Author: Tobias Naegeli + * Lorenz Meier + * + * 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 + +struct attitude_estimator_ekf_params { + float r[9]; + float q[12]; +}; + +struct attitude_estimator_ekf_param_handles { + param_t r0, r1, r2, r3, r4, r5, r6, r7, r8; + param_t q0, q1, q2, q3, q4, q5, q6, q7, q8, q9, q10, q11; +}; + +/** + * 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);