Commit 7afaa6eb authored by Olivier Stasse's avatar Olivier Stasse
Browse files

[filters] Add Madgwickahrs.

Reimplementing this part to drop the GPL license.
parent 9c2b67fc
......@@ -58,12 +58,29 @@
namespace dynamicgraph {
namespace sot {
namespace talos_balance {
/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */
/* --------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------- */
/* --------------------------------------------------------------- */
/** \addtogroup Filters
This class implements the MadgwickAHRS filter as described
in http://x-io.co.uk/res/doc/madgwick_internal_report.pdf
This method uses a gradient descent approach to compute the orientation
from an IMU.
The signals input are:
<ul>
<li>m_accelerometerSIN: \f$[a_x, a_y, a_z]^T\f$ in \f$m.s^{-2}\f$</li>
<li>m_gyroscopeSIN: \f$[g_x, g_y, g_z]^T\f$ in \f$rad.s^{-1}\f$</li>
<li>m_imu_quatSOUT: \f$[q_0, q_1, q_2, q_3]^T</li> estimated rotation as a quaternion</li>
</ul>
The internal parameters are:
<ul>
<li>\f$Beta\f$: Gradient step weight (default to 0.01) </li>
<li>\f$m_sampleFref\f$: Sampling Frequency computed from the control
period when using init.</li>
</ul>
*/
class SOTMADGWICKAHRS_EXPORT MadgwickAHRS
:public::dynamicgraph::Entity
{
......@@ -80,9 +97,12 @@ namespace dynamicgraph {
void set_beta(const double & beta);
/* --- SIGNALS --- */
DECLARE_SIGNAL_IN(accelerometer, dynamicgraph::Vector); /// ax ay az in m.s-2
DECLARE_SIGNAL_IN(gyroscope, dynamicgraph::Vector); /// gx gy gz in rad.s-1
DECLARE_SIGNAL_OUT(imu_quat, dynamicgraph::Vector); /// Estimated orientation of IMU as a quaternion
/// ax ay az in m.s-2
DECLARE_SIGNAL_IN(accelerometer, dynamicgraph::Vector);
/// gx gy gz in rad.s-1
DECLARE_SIGNAL_IN(gyroscope, dynamicgraph::Vector);
/// Estimated orientation of IMU as a quaternion
DECLARE_SIGNAL_OUT(imu_quat, dynamicgraph::Vector);
protected:
/* --- COMMANDS --- */
......@@ -91,17 +111,20 @@ namespace dynamicgraph {
/* --- METHODS --- */
double invSqrt(double x);
void madgwickAHRSupdateIMU(double gx, double gy, double gz, double ax, double ay, double az) ;
//void madgwickAHRSupdate(double gx, double gy, double gz, double ax, double ay, double az, double mx, double my, double mz);
void madgwickAHRSupdateIMU
(double gx, double gy, double gz, double ax, double ay, double az) ;
protected:
bool m_initSucceeded; /// true if the entity has been successfully initialized
double m_beta; /// 2 * proportional gain (Kp)
double m_q0, m_q1, m_q2, m_q3; /// quaternion of sensor frame
double m_sampleFreq; /// sample frequency in Hz
/// true if the entity has been successfully initialized
bool m_initSucceeded;
/// 2 * proportional gain (Kp)
double m_beta;
/// quaternion of sensor frame
double m_q0, m_q1, m_q2, m_q3;
/// sample frequency in Hz
double m_sampleFreq;
}; // class MadgwickAHRS
} // namespace talos_balance
} // namespace sot
} // namespace dynamicgraph
......
......@@ -23,8 +23,6 @@
namespace dynamicgraph
{
namespace sot
{
namespace talos_balance
{
namespace dg = ::dynamicgraph;
using namespace dg;
......@@ -164,7 +162,7 @@ namespace dynamicgraph
double recipNorm;
double s0, s1, s2, s3;
double qDot1, qDot2, qDot3, qDot4;
double _2q0, _2q1, _2q2, _2q3, _4q0, _4q1, _4q2 ,_8q1, _8q2;
double o2q0, o2q1, o2q2, o2q3, o4q0, o4q1, o4q2 ,o8q1, o8q2;
double q0q0, q1q1, q2q2, q3q3;
// Rate of change of quaternion from gyroscope
......@@ -184,27 +182,27 @@ namespace dynamicgraph
az *= recipNorm;
// Auxiliary variables to avoid repeated arithmetic
_2q0 = 2.0 * m_q0;
_2q1 = 2.0 * m_q1;
_2q2 = 2.0 * m_q2;
_2q3 = 2.0 * m_q3;
_4q0 = 4.0 * m_q0;
_4q1 = 4.0 * m_q1;
_4q2 = 4.0 * m_q2;
_8q1 = 8.0 * m_q1;
_8q2 = 8.0 * m_q2;
o2q0 = 2.0 * m_q0;
o2q1 = 2.0 * m_q1;
o2q2 = 2.0 * m_q2;
o2q3 = 2.0 * m_q3;
o4q0 = 4.0 * m_q0;
o4q1 = 4.0 * m_q1;
o4q2 = 4.0 * m_q2;
o8q1 = 8.0 * m_q1;
o8q2 = 8.0 * m_q2;
q0q0 = m_q0 * m_q0;
q1q1 = m_q1 * m_q1;
q2q2 = m_q2 * m_q2;
q3q3 = m_q3 * m_q3;
// Gradient decent algorithm corrective step
s0 = _4q0 * q2q2 + _2q2 * ax + _4q0 * q1q1 - _2q1 * ay;
s1 = _4q1 * q3q3 - _2q3 * ax + 4.0 * q0q0 * m_q1 - _2q0 * ay -
_4q1 + _8q1 * q1q1 + _8q1 * q2q2 + _4q1 * az;
s2 = 4.0 * q0q0 * m_q2 + _2q0 * ax + _4q2 * q3q3 - _2q3 * ay -
_4q2 + _8q2 * q1q1 + _8q2 * q2q2 + _4q2 * az;
s3 = 4.0 * q1q1 * m_q3 - _2q1 * ax + 4.0 * q2q2 * m_q3 - _2q2 * ay;
s0 = o4q0 * q2q2 + o2q2 * ax + o4q0 * q1q1 - o2q1 * ay;
s1 = o4q1 * q3q3 - o2q3 * ax + 4.0 * q0q0 * m_q1 - o2q0 * ay -
o4q1 + o8q1 * q1q1 + o8q1 * q2q2 + o4q1 * az;
s2 = 4.0 * q0q0 * m_q2 + o2q0 * ax + o4q2 * q3q3 - o2q3 * ay -
o4q2 + o8q2 * q1q1 + o8q2 * q2q2 + o4q2 * az;
s3 = 4.0 * q1q1 * m_q3 - o2q1 * ax + 4.0 * q2q2 * m_q3 - o2q2 * ay;
if(!((s0 == 0.0) && (s1 == 0.0) && (s2 == 0.0) && (s3 == 0.0)))
{
// normalise step magnitude
......@@ -251,7 +249,6 @@ namespace dynamicgraph
}
catch (ExceptionSignal e) {}
}
} // namespace talos_balance
} // namespace sot
} // namespace dynamicgraph
......
......@@ -56,6 +56,11 @@ SET(TEST_test_control_pd_LIBS
SET(TEST_test_filter_differentiator_LIBS
filter-differentiator
)
SET(TEST_test_madgwick_ahrs_LIBS
madgwickahrs
)
#test paths and names (without .cpp extension)
SET (tests
dummy
......@@ -63,6 +68,7 @@ SET (tests
control/test_control_pd
filters/test_filter_differentiator
filters/test_madgwick_ahrs
signal/test_signal
signal/test_depend
......
/*
* Copyright 2019,
* Olivier Stasse,
*
* CNRS/AIST
*
*/
#include <iostream>
#include <sot/core/debug.hh>
#ifndef WIN32
#include <unistd.h>
#endif
using namespace std;
#include <dynamic-graph/factory.h>
#include <dynamic-graph/entity.h>
#include <sot/core/madgwickahrs.hh>
#include <sstream>
using namespace dynamicgraph;
using namespace dynamicgraph::sot;
#define BOOST_TEST_MODULE test-filter-differentiator
#include <boost/test/unit_test.hpp>
#include <boost/test/output_test_stream.hpp>
using boost::test_tools::output_test_stream;
BOOST_AUTO_TEST_CASE(test_filter_differentiator)
{
sot::MadgwickAHRS *aFilter = new
MadgwickAHRS("MadgwickAHRS");
double timestep=0.001,beta=0.01;
aFilter->init(timestep);
aFilter->set_beta(beta);
srand(0);
dynamicgraph::Vector acc(3);
dynamicgraph::Vector angvel(3);
acc(0)=0.3; acc(1)=0.2; acc(2)=0.3;
aFilter->m_accelerometerSIN = acc;
angvel(0)=0.1;angvel(1)=-0.1;angvel(2)=0.3;
aFilter->m_gyroscopeSIN = angvel;
aFilter->m_imu_quatSOUT.recompute(0);
output_test_stream output;
ostringstream anoss;
aFilter->m_imu_quatSOUT.get(output);
aFilter->m_imu_quatSOUT.get(anoss);
BOOST_CHECK(output.is_equal(" 1\n"
" 5.5547e-05\n"
"-5.83205e-05\n"
" 0.00015\n"));
}
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment