forked from Archive/PX4-Autopilot
SO3 estimator code has been cleaned
This commit is contained in:
parent
bcd745fb0d
commit
b3f1adc54b
|
@ -69,12 +69,13 @@ MODULES += modules/mavlink_onboard
|
|||
MODULES += modules/gpio_led
|
||||
|
||||
#
|
||||
# Estimation modules (EKF / other filters)
|
||||
# Estimation modules (EKF/ SO3 / other filters)
|
||||
#
|
||||
#MODULES += modules/attitude_estimator_ekf
|
||||
MODULES += modules/att_pos_estimator_ekf
|
||||
#MODULES += modules/position_estimator_inav
|
||||
MODULES += examples/flow_position_estimator
|
||||
MODULES += modules/attitude_estimator_so3
|
||||
|
||||
#
|
||||
# Vehicle Control
|
||||
|
|
|
@ -69,10 +69,10 @@ MODULES += modules/mavlink_onboard
|
|||
MODULES += modules/gpio_led
|
||||
|
||||
#
|
||||
# Estimation modules (EKF / other filters)
|
||||
# Estimation modules (EKF/ SO3 / other filters)
|
||||
#
|
||||
MODULES += modules/attitude_estimator_ekf
|
||||
MODULES += modules/attitude_estimator_so3_comp
|
||||
MODULES += modules/attitude_estimator_so3
|
||||
MODULES += modules/att_pos_estimator_ekf
|
||||
MODULES += modules/position_estimator_inav
|
||||
MODULES += examples/flow_position_estimator
|
||||
|
|
|
@ -68,9 +68,10 @@ MODULES += modules/mavlink
|
|||
MODULES += modules/mavlink_onboard
|
||||
|
||||
#
|
||||
# Estimation modules (EKF / other filters)
|
||||
# Estimation modules (EKF/ SO3 / other filters)
|
||||
#
|
||||
MODULES += modules/attitude_estimator_ekf
|
||||
MODULES += modules/attitude_estimator_so3
|
||||
MODULES += modules/att_pos_estimator_ekf
|
||||
MODULES += modules/position_estimator_inav
|
||||
MODULES += examples/flow_position_estimator
|
||||
|
|
|
@ -1,16 +1,49 @@
|
|||
/*
|
||||
* Author: Hyon Lim <limhyon@gmail.com, hyonlim@snu.ac.kr>
|
||||
/****************************************************************************
|
||||
*
|
||||
* @file attitude_estimator_so3_comp_main.c
|
||||
* Copyright (C) 2013 PX4 Development Team. All rights reserved.
|
||||
* Author: Hyon Lim <limhyon@gmail.com>
|
||||
* Anton Babushkin <anton.babushkin@me.com>
|
||||
*
|
||||
* 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_so3_main.cpp
|
||||
*
|
||||
* Implementation of nonlinear complementary filters on the SO(3).
|
||||
* This code performs attitude estimation by using accelerometer, gyroscopes and magnetometer.
|
||||
* Result is provided as quaternion, 1-2-3 Euler angle and rotation matrix.
|
||||
*
|
||||
*
|
||||
* Theory of nonlinear complementary filters on the SO(3) is based on [1].
|
||||
* Quaternion realization of [1] is based on [2].
|
||||
* Optmized quaternion update code is based on Sebastian Madgwick's implementation.
|
||||
*
|
||||
*
|
||||
* References
|
||||
* [1] Mahony, R.; Hamel, T.; Pflimlin, Jean-Michel, "Nonlinear Complementary Filters on the Special Orthogonal Group," Automatic Control, IEEE Transactions on , vol.53, no.5, pp.1203,1218, June 2008
|
||||
* [2] Euston, M.; Coote, P.; Mahony, R.; Jonghyuk Kim; Hamel, T., "A complementary filter for attitude estimation of a fixed-wing UAV," Intelligent Robots and Systems, 2008. IROS 2008. IEEE/RSJ International Conference on , vol., no., pp.340,345, 22-26 Sept. 2008
|
||||
|
@ -46,16 +79,16 @@
|
|||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
#include "attitude_estimator_so3_comp_params.h"
|
||||
#include "attitude_estimator_so3_params.h"
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
|
||||
extern "C" __EXPORT int attitude_estimator_so3_comp_main(int argc, char *argv[]);
|
||||
extern "C" __EXPORT int attitude_estimator_so3_main(int argc, char *argv[]);
|
||||
|
||||
static bool thread_should_exit = false; /**< Deamon exit flag */
|
||||
static bool thread_running = false; /**< Deamon status flag */
|
||||
static int attitude_estimator_so3_comp_task; /**< Handle of deamon task / thread */
|
||||
static int attitude_estimator_so3_task; /**< Handle of deamon task / thread */
|
||||
|
||||
//! Auxiliary variables to reduce number of repeated operations
|
||||
static float q0 = 1.0f, q1 = 0.0f, q2 = 0.0f, q3 = 0.0f; /** quaternion of sensor frame relative to auxiliary frame */
|
||||
|
@ -68,9 +101,9 @@ static float q3q3;
|
|||
static bool bFilterInit = false;
|
||||
|
||||
/**
|
||||
* Mainloop of attitude_estimator_so3_comp.
|
||||
* Mainloop of attitude_estimator_so3.
|
||||
*/
|
||||
int attitude_estimator_so3_comp_thread_main(int argc, char *argv[]);
|
||||
int attitude_estimator_so3_thread_main(int argc, char *argv[]);
|
||||
|
||||
/**
|
||||
* Print the correct usage.
|
||||
|
@ -88,19 +121,19 @@ usage(const char *reason)
|
|||
if (reason)
|
||||
fprintf(stderr, "%s\n", reason);
|
||||
|
||||
fprintf(stderr, "usage: attitude_estimator_so3_comp {start|stop|status}\n");
|
||||
fprintf(stderr, "usage: attitude_estimator_so3 {start|stop|status}\n");
|
||||
exit(1);
|
||||
}
|
||||
|
||||
/**
|
||||
* The attitude_estimator_so3_comp app only briefly exists to start
|
||||
* The attitude_estimator_so3 app only briefly exists to start
|
||||
* the background job. The stack size assigned in the
|
||||
* Makefile does only apply to this management task.
|
||||
*
|
||||
* The actual stack size should be set in the call
|
||||
* to task_create().
|
||||
*/
|
||||
int attitude_estimator_so3_comp_main(int argc, char *argv[])
|
||||
int attitude_estimator_so3_main(int argc, char *argv[])
|
||||
{
|
||||
if (argc < 1)
|
||||
usage("missing command");
|
||||
|
@ -114,11 +147,11 @@ int attitude_estimator_so3_comp_main(int argc, char *argv[])
|
|||
}
|
||||
|
||||
thread_should_exit = false;
|
||||
attitude_estimator_so3_comp_task = task_spawn_cmd("attitude_estimator_so3_comp",
|
||||
attitude_estimator_so3_task = task_spawn_cmd("attitude_estimator_so3",
|
||||
SCHED_DEFAULT,
|
||||
SCHED_PRIORITY_MAX - 5,
|
||||
14000,
|
||||
attitude_estimator_so3_comp_thread_main,
|
||||
attitude_estimator_so3_thread_main,
|
||||
(argv) ? (const char **)&argv[2] : (const char **)NULL);
|
||||
exit(0);
|
||||
}
|
||||
|
@ -356,7 +389,7 @@ void NonlinearSO3AHRSupdate(float gx, float gy, float gz, float ax, float ay, fl
|
|||
* @param argc number of commandline arguments (plus command name)
|
||||
* @param argv strings containing the arguments
|
||||
*/
|
||||
int attitude_estimator_so3_comp_thread_main(int argc, char *argv[])
|
||||
int attitude_estimator_so3_thread_main(int argc, char *argv[])
|
||||
{
|
||||
const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds
|
||||
|
||||
|
@ -415,8 +448,8 @@ int attitude_estimator_so3_comp_thread_main(int argc, char *argv[])
|
|||
uint32_t sensor_last_count[3] = {0, 0, 0};
|
||||
uint64_t sensor_last_timestamp[3] = {0, 0, 0};
|
||||
|
||||
struct attitude_estimator_so3_comp_params so3_comp_params;
|
||||
struct attitude_estimator_so3_comp_param_handles so3_comp_param_handles;
|
||||
struct attitude_estimator_so3_params so3_comp_params;
|
||||
struct attitude_estimator_so3_param_handles so3_comp_param_handles;
|
||||
|
||||
/* initialize parameter handles */
|
||||
parameters_init(&so3_comp_param_handles);
|
||||
|
@ -430,7 +463,7 @@ int attitude_estimator_so3_comp_thread_main(int argc, char *argv[])
|
|||
unsigned offset_count = 0;
|
||||
|
||||
/* register the perf counter */
|
||||
perf_counter_t so3_comp_loop_perf = perf_alloc(PC_ELAPSED, "attitude_estimator_so3_comp");
|
||||
perf_counter_t so3_comp_loop_perf = perf_alloc(PC_ELAPSED, "attitude_estimator_so3");
|
||||
|
||||
/* Main loop*/
|
||||
while (!thread_should_exit) {
|
|
@ -0,0 +1,86 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2013 PX4 Development Team. All rights reserved.
|
||||
* Author: Hyon Lim <limhyon@gmail.com>
|
||||
* Anton Babushkin <anton.babushkin@me.com>
|
||||
*
|
||||
* 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_so3_params.c
|
||||
*
|
||||
* Parameters for nonlinear complementary filters on the SO(3).
|
||||
*/
|
||||
|
||||
#include "attitude_estimator_so3_params.h"
|
||||
|
||||
/* This is filter gain for nonlinear SO3 complementary filter */
|
||||
/* NOTE : How to tune the gain? First of all, stick with this default gain. And let the quad in stable place.
|
||||
Log the steady state reponse of filter. If it is too slow, increase SO3_COMP_KP.
|
||||
If you are flying from ground to high altitude in short amount of time, please increase SO3_COMP_KI which
|
||||
will compensate gyro bias which depends on temperature and vibration of your vehicle */
|
||||
PARAM_DEFINE_FLOAT(SO3_COMP_KP, 1.0f); //! This parameter will give you about 15 seconds convergence time.
|
||||
//! You can set this gain higher if you want more fast response.
|
||||
//! But note that higher gain will give you also higher overshoot.
|
||||
PARAM_DEFINE_FLOAT(SO3_COMP_KI, 0.05f); //! This gain will incorporate slow time-varying bias (e.g., temperature change)
|
||||
//! This gain is depend on your vehicle status.
|
||||
|
||||
/* offsets in roll, pitch and yaw of sensor plane and body */
|
||||
PARAM_DEFINE_FLOAT(SO3_ROLL_OFFS, 0.0f);
|
||||
PARAM_DEFINE_FLOAT(SO3_PITCH_OFFS, 0.0f);
|
||||
PARAM_DEFINE_FLOAT(SO3_YAW_OFFS, 0.0f);
|
||||
|
||||
int parameters_init(struct attitude_estimator_so3_param_handles *h)
|
||||
{
|
||||
/* Filter gain parameters */
|
||||
h->Kp = param_find("SO3_COMP_KP");
|
||||
h->Ki = param_find("SO3_COMP_KI");
|
||||
|
||||
/* Attitude offset (WARNING: Do not change if you do not know what exactly this variable wil lchange) */
|
||||
h->roll_off = param_find("SO3_ROLL_OFFS");
|
||||
h->pitch_off = param_find("SO3_PITCH_OFFS");
|
||||
h->yaw_off = param_find("SO3_YAW_OFFS");
|
||||
|
||||
return OK;
|
||||
}
|
||||
|
||||
int parameters_update(const struct attitude_estimator_so3_param_handles *h, struct attitude_estimator_so3_params *p)
|
||||
{
|
||||
/* Update filter gain */
|
||||
param_get(h->Kp, &(p->Kp));
|
||||
param_get(h->Ki, &(p->Ki));
|
||||
|
||||
/* Update attitude offset */
|
||||
param_get(h->roll_off, &(p->roll_off));
|
||||
param_get(h->pitch_off, &(p->pitch_off));
|
||||
param_get(h->yaw_off, &(p->yaw_off));
|
||||
|
||||
return OK;
|
||||
}
|
|
@ -0,0 +1,67 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2013 PX4 Development Team. All rights reserved.
|
||||
* Author: Hyon Lim <limhyon@gmail.com>
|
||||
* Anton Babushkin <anton.babushkin@me.com>
|
||||
*
|
||||
* 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_so3_params.h
|
||||
*
|
||||
* Parameters for nonlinear complementary filters on the SO(3).
|
||||
*/
|
||||
|
||||
#include <systemlib/param/param.h>
|
||||
|
||||
struct attitude_estimator_so3_params {
|
||||
float Kp;
|
||||
float Ki;
|
||||
float roll_off;
|
||||
float pitch_off;
|
||||
float yaw_off;
|
||||
};
|
||||
|
||||
struct attitude_estimator_so3_param_handles {
|
||||
param_t Kp, Ki;
|
||||
param_t roll_off, pitch_off, yaw_off;
|
||||
};
|
||||
|
||||
/**
|
||||
* Initialize all parameter handles and values
|
||||
*
|
||||
*/
|
||||
int parameters_init(struct attitude_estimator_so3_param_handles *h);
|
||||
|
||||
/**
|
||||
* Update all parameters
|
||||
*
|
||||
*/
|
||||
int parameters_update(const struct attitude_estimator_so3_param_handles *h, struct attitude_estimator_so3_params *p);
|
|
@ -0,0 +1,8 @@
|
|||
#
|
||||
# Attitude estimator (Nonlinear SO(3) complementary Filter)
|
||||
#
|
||||
|
||||
MODULE_COMMAND = attitude_estimator_so3
|
||||
|
||||
SRCS = attitude_estimator_so3_main.cpp \
|
||||
attitude_estimator_so3_params.c
|
|
@ -1,63 +0,0 @@
|
|||
/*
|
||||
* Author: Hyon Lim <limhyon@gmail.com, hyonlim@snu.ac.kr>
|
||||
*
|
||||
* @file attitude_estimator_so3_comp_params.c
|
||||
*
|
||||
* Implementation of nonlinear complementary filters on the SO(3).
|
||||
* This code performs attitude estimation by using accelerometer, gyroscopes and magnetometer.
|
||||
* Result is provided as quaternion, 1-2-3 Euler angle and rotation matrix.
|
||||
*
|
||||
* Theory of nonlinear complementary filters on the SO(3) is based on [1].
|
||||
* Quaternion realization of [1] is based on [2].
|
||||
* Optmized quaternion update code is based on Sebastian Madgwick's implementation.
|
||||
*
|
||||
* References
|
||||
* [1] Mahony, R.; Hamel, T.; Pflimlin, Jean-Michel, "Nonlinear Complementary Filters on the Special Orthogonal Group," Automatic Control, IEEE Transactions on , vol.53, no.5, pp.1203,1218, June 2008
|
||||
* [2] Euston, M.; Coote, P.; Mahony, R.; Jonghyuk Kim; Hamel, T., "A complementary filter for attitude estimation of a fixed-wing UAV," Intelligent Robots and Systems, 2008. IROS 2008. IEEE/RSJ International Conference on , vol., no., pp.340,345, 22-26 Sept. 2008
|
||||
*/
|
||||
|
||||
#include "attitude_estimator_so3_comp_params.h"
|
||||
|
||||
/* This is filter gain for nonlinear SO3 complementary filter */
|
||||
/* NOTE : How to tune the gain? First of all, stick with this default gain. And let the quad in stable place.
|
||||
Log the steady state reponse of filter. If it is too slow, increase SO3_COMP_KP.
|
||||
If you are flying from ground to high altitude in short amount of time, please increase SO3_COMP_KI which
|
||||
will compensate gyro bias which depends on temperature and vibration of your vehicle */
|
||||
PARAM_DEFINE_FLOAT(SO3_COMP_KP, 1.0f); //! This parameter will give you about 15 seconds convergence time.
|
||||
//! You can set this gain higher if you want more fast response.
|
||||
//! But note that higher gain will give you also higher overshoot.
|
||||
PARAM_DEFINE_FLOAT(SO3_COMP_KI, 0.05f); //! This gain will incorporate slow time-varying bias (e.g., temperature change)
|
||||
//! This gain is depend on your vehicle status.
|
||||
|
||||
/* offsets in roll, pitch and yaw of sensor plane and body */
|
||||
PARAM_DEFINE_FLOAT(ATT_ROLL_OFFS, 0.0f);
|
||||
PARAM_DEFINE_FLOAT(ATT_PITCH_OFFS, 0.0f);
|
||||
PARAM_DEFINE_FLOAT(ATT_YAW_OFFS, 0.0f);
|
||||
|
||||
int parameters_init(struct attitude_estimator_so3_comp_param_handles *h)
|
||||
{
|
||||
/* Filter gain parameters */
|
||||
h->Kp = param_find("SO3_COMP_KP");
|
||||
h->Ki = param_find("SO3_COMP_KI");
|
||||
|
||||
/* Attitude offset (WARNING: Do not change if you do not know what exactly this variable wil lchange) */
|
||||
h->roll_off = param_find("ATT_ROLL_OFFS");
|
||||
h->pitch_off = param_find("ATT_PITCH_OFFS");
|
||||
h->yaw_off = param_find("ATT_YAW_OFFS");
|
||||
|
||||
return OK;
|
||||
}
|
||||
|
||||
int parameters_update(const struct attitude_estimator_so3_comp_param_handles *h, struct attitude_estimator_so3_comp_params *p)
|
||||
{
|
||||
/* Update filter gain */
|
||||
param_get(h->Kp, &(p->Kp));
|
||||
param_get(h->Ki, &(p->Ki));
|
||||
|
||||
/* Update attitude offset */
|
||||
param_get(h->roll_off, &(p->roll_off));
|
||||
param_get(h->pitch_off, &(p->pitch_off));
|
||||
param_get(h->yaw_off, &(p->yaw_off));
|
||||
|
||||
return OK;
|
||||
}
|
|
@ -1,44 +0,0 @@
|
|||
/*
|
||||
* Author: Hyon Lim <limhyon@gmail.com, hyonlim@snu.ac.kr>
|
||||
*
|
||||
* @file attitude_estimator_so3_comp_params.h
|
||||
*
|
||||
* Implementation of nonlinear complementary filters on the SO(3).
|
||||
* This code performs attitude estimation by using accelerometer, gyroscopes and magnetometer.
|
||||
* Result is provided as quaternion, 1-2-3 Euler angle and rotation matrix.
|
||||
*
|
||||
* Theory of nonlinear complementary filters on the SO(3) is based on [1].
|
||||
* Quaternion realization of [1] is based on [2].
|
||||
* Optmized quaternion update code is based on Sebastian Madgwick's implementation.
|
||||
*
|
||||
* References
|
||||
* [1] Mahony, R.; Hamel, T.; Pflimlin, Jean-Michel, "Nonlinear Complementary Filters on the Special Orthogonal Group," Automatic Control, IEEE Transactions on , vol.53, no.5, pp.1203,1218, June 2008
|
||||
* [2] Euston, M.; Coote, P.; Mahony, R.; Jonghyuk Kim; Hamel, T., "A complementary filter for attitude estimation of a fixed-wing UAV," Intelligent Robots and Systems, 2008. IROS 2008. IEEE/RSJ International Conference on , vol., no., pp.340,345, 22-26 Sept. 2008
|
||||
*/
|
||||
|
||||
#include <systemlib/param/param.h>
|
||||
|
||||
struct attitude_estimator_so3_comp_params {
|
||||
float Kp;
|
||||
float Ki;
|
||||
float roll_off;
|
||||
float pitch_off;
|
||||
float yaw_off;
|
||||
};
|
||||
|
||||
struct attitude_estimator_so3_comp_param_handles {
|
||||
param_t Kp, Ki;
|
||||
param_t roll_off, pitch_off, yaw_off;
|
||||
};
|
||||
|
||||
/**
|
||||
* Initialize all parameter handles and values
|
||||
*
|
||||
*/
|
||||
int parameters_init(struct attitude_estimator_so3_comp_param_handles *h);
|
||||
|
||||
/**
|
||||
* Update all parameters
|
||||
*
|
||||
*/
|
||||
int parameters_update(const struct attitude_estimator_so3_comp_param_handles *h, struct attitude_estimator_so3_comp_params *p);
|
|
@ -1,8 +0,0 @@
|
|||
#
|
||||
# Attitude estimator (Nonlinear SO3 complementary Filter)
|
||||
#
|
||||
|
||||
MODULE_COMMAND = attitude_estimator_so3_comp
|
||||
|
||||
SRCS = attitude_estimator_so3_comp_main.cpp \
|
||||
attitude_estimator_so3_comp_params.c
|
Loading…
Reference in New Issue