Skip to content
GitLab
Commit 28178759 authored by Olivier Stasse's avatar Olivier Stasse Committed by olivier stasse
Browse files

[filters] Add Madgwickahrs. 

Reimplementing this part to drop the GPL license.
parent debe436e
Hide whitespace changes
Inline Side-by-side
include/sot/core/madgwickahrs.hh
View file @ 28178759
......@@ -58,50 +58,73 @@
namespace dynamicgraph {
namespace sot {
namespace talos_balance {
/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */
class SOTMADGWICKAHRS_EXPORT MadgwickAHRS
:public::dynamicgraph::Entity
{
typedef MadgwickAHRS EntityClassName;
DYNAMIC_GRAPH_ENTITY_DECL();
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
/* --------------------------------------------------------------- */
/* --- 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
{
typedef MadgwickAHRS EntityClassName;
DYNAMIC_GRAPH_ENTITY_DECL();
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
/* --- CONSTRUCTOR ---- */
MadgwickAHRS( const std::string & name );
void init(const double& dt);
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
protected:
/* --- COMMANDS --- */
/* --- ENTITY INHERITANCE --- */
virtual void display( std::ostream& os ) const;
/* --- 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);
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
}; // class MadgwickAHRS
} // namespace talos_balance
void init(const double& dt);
void set_beta(const double & beta);
/* --- SIGNALS --- */
/// 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 --- */
/* --- ENTITY INHERITANCE --- */
virtual void display( std::ostream& os ) const;
/* --- METHODS --- */
double invSqrt(double x);
void madgwickAHRSupdateIMU
(double gx, double gy, double gz, double ax, double ay, double az) ;
protected:
/// 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 sot
} // namespace dynamicgraph
......
src/filters/madgwickahrs.cpp
View file @ 28178759
......@@ -24,158 +24,156 @@ namespace dynamicgraph
{
namespace sot
{
namespace talos_balance
{
namespace dg = ::dynamicgraph;
using namespace dg;
using namespace dg::command;
using namespace std;
namespace dg = ::dynamicgraph;
using namespace dg;
using namespace dg::command;
using namespace std;
typedef Vector6d Vector6;
typedef Vector6d Vector6;
#define PROFILE_MADGWICKAHRS_COMPUTATION "MadgwickAHRS computation"
#define INPUT_SIGNALS m_accelerometerSIN << m_gyroscopeSIN
#define OUTPUT_SIGNALS m_imu_quatSOUT
/// Define EntityClassName here rather than in the header file
/// so that it can be used by the macros DEFINE_SIGNAL_**_FUNCTION.
typedef MadgwickAHRS EntityClassName;
/* --- DG FACTORY ---------------------------------------------------- */
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(MadgwickAHRS,
"MadgwickAHRS");
/* ------------------------------------------------------------------- */
/* --- CONSTRUCTION -------------------------------------------------- */
/* ------------------------------------------------------------------- */
MadgwickAHRS::MadgwickAHRS(const std::string& name)
: Entity(name)
,CONSTRUCT_SIGNAL_IN( accelerometer, dynamicgraph::Vector)
,CONSTRUCT_SIGNAL_IN( gyroscope, dynamicgraph::Vector)
,CONSTRUCT_SIGNAL_OUT(imu_quat, dynamicgraph::Vector,
m_gyroscopeSIN <<
m_accelerometerSIN)
,m_initSucceeded(false)
,m_beta(betaDef)
,m_q0(1.0)
,m_q1(0.0)
,m_q2(0.0)
,m_q3(0.0)
,m_sampleFreq(512.0)
{
Entity::signalRegistration( INPUT_SIGNALS << OUTPUT_SIGNALS );
/* Commands. */
addCommand
("init",
makeCommandVoid1
(*this, &MadgwickAHRS::init,
docCommandVoid1
("Initialize the entity.",
"Timestep in seconds (double)")));
addCommand
("getBeta",
makeDirectGetter
(*this, &m_beta,
docDirectGetter("Beta parameter", "double")));
addCommand
("setBeta",
makeCommandVoid1
(*this, &MadgwickAHRS::set_beta,
docCommandVoid1("Set the filter parameter beta", "double")));
}
void MadgwickAHRS::init(const double& dt)
{
if(dt<=0.0)
return SEND_MSG("Timestep must be positive", MSG_TYPE_ERROR);
m_sampleFreq=1.0/dt;
m_initSucceeded = true;
}
/// Define EntityClassName here rather than in the header file
/// so that it can be used by the macros DEFINE_SIGNAL_**_FUNCTION.
typedef MadgwickAHRS EntityClassName;
/* --- DG FACTORY ---------------------------------------------------- */
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(MadgwickAHRS,
"MadgwickAHRS");
/* ------------------------------------------------------------------- */
/* --- CONSTRUCTION -------------------------------------------------- */
/* ------------------------------------------------------------------- */
MadgwickAHRS::MadgwickAHRS(const std::string& name)
: Entity(name)
,CONSTRUCT_SIGNAL_IN( accelerometer, dynamicgraph::Vector)
,CONSTRUCT_SIGNAL_IN( gyroscope, dynamicgraph::Vector)
,CONSTRUCT_SIGNAL_OUT(imu_quat, dynamicgraph::Vector,
m_gyroscopeSIN <<
m_accelerometerSIN)
,m_initSucceeded(false)
,m_beta(betaDef)
,m_q0(1.0)
,m_q1(0.0)
,m_q2(0.0)
,m_q3(0.0)
,m_sampleFreq(512.0)
{
Entity::signalRegistration( INPUT_SIGNALS << OUTPUT_SIGNALS );
/* Commands. */
addCommand
("init",
makeCommandVoid1
(*this, &MadgwickAHRS::init,
docCommandVoid1
("Initialize the entity.",
"Timestep in seconds (double)")));
addCommand
("getBeta",
makeDirectGetter
(*this, &m_beta,
docDirectGetter("Beta parameter", "double")));
addCommand
("setBeta",
makeCommandVoid1
(*this, &MadgwickAHRS::set_beta,
docCommandVoid1("Set the filter parameter beta", "double")));
}
void MadgwickAHRS::init(const double& dt)
{
if(dt<=0.0)
return SEND_MSG("Timestep must be positive", MSG_TYPE_ERROR);
m_sampleFreq=1.0/dt;
m_initSucceeded = true;
}
void MadgwickAHRS::set_beta(const double& beta)
{
if(beta<0.0 || beta>1.0)
return SEND_MSG("Beta must be in [0,1]", MSG_TYPE_ERROR);
m_beta = beta;
}
void MadgwickAHRS::set_beta(const double& beta)
{
if(beta<0.0 || beta>1.0)
return SEND_MSG("Beta must be in [0,1]", MSG_TYPE_ERROR);
m_beta = beta;
}
/* ------------------------------------------------------------------- */
/* --- SIGNALS ------------------------------------------------------- */
/* ------------------------------------------------------------------- */
/* ------------------------------------------------------------------- */
/* --- SIGNALS ------------------------------------------------------- */
/* ------------------------------------------------------------------- */
DEFINE_SIGNAL_OUT_FUNCTION(imu_quat, dynamicgraph::Vector)
{
if(!m_initSucceeded)
DEFINE_SIGNAL_OUT_FUNCTION(imu_quat, dynamicgraph::Vector)
{
if(!m_initSucceeded)
{
SEND_WARNING_STREAM_MSG
("Cannot compute signal imu_quat before initialization!");
return s;
}
const dynamicgraph::Vector& accelerometer = m_accelerometerSIN(iter);
const dynamicgraph::Vector& gyroscope = m_gyroscopeSIN(iter);
const dynamicgraph::Vector& accelerometer = m_accelerometerSIN(iter);
const dynamicgraph::Vector& gyroscope = m_gyroscopeSIN(iter);
getProfiler().start(PROFILE_MADGWICKAHRS_COMPUTATION);
{
// Update state with new measurment
madgwickAHRSupdateIMU
( gyroscope(0), gyroscope(1), gyroscope(2),
accelerometer(0), accelerometer(1), accelerometer(2));
if(s.size()!=4)
s.resize(4);
s(0) = m_q0;
s(1) = m_q1;
s(2) = m_q2;
s(3) = m_q3;
}
getProfiler().stop(PROFILE_MADGWICKAHRS_COMPUTATION);
return s;
getProfiler().start(PROFILE_MADGWICKAHRS_COMPUTATION);
{
// Update state with new measurment
madgwickAHRSupdateIMU
( gyroscope(0), gyroscope(1), gyroscope(2),
accelerometer(0), accelerometer(1), accelerometer(2));
if(s.size()!=4)
s.resize(4);
s(0) = m_q0;
s(1) = m_q1;
s(2) = m_q2;
s(3) = m_q3;
}
getProfiler().stop(PROFILE_MADGWICKAHRS_COMPUTATION);
return s;
}
/* --- COMMANDS ------------------------------------------------------ */
/* ------------------------------------------------------------------- */
// ************************ PROTECTED MEMBER METHODS ********************
/* ------------------------------------------------------------------- */
/* --- COMMANDS ------------------------------------------------------ */
// Fast inverse square-root
// See: http://en.wikipedia.org/wiki/Fast_inverse_square_root
double MadgwickAHRS::invSqrt(double x)
{
/*
float halfx = 0.5f * x;
float y = x;
long i = *(long*)&y;
i = 0x5f3759df - (i>>1);
y = *(float*)&i;
y = y * (1.5f - (halfx * y * y));
return y;*/
return (1.0/sqrt(x)); //we're not in the 70's
}
/* ------------------------------------------------------------------- */
// ************************ PROTECTED MEMBER METHODS ********************
/* ------------------------------------------------------------------- */
// IMU algorithm update
void MadgwickAHRS::
madgwickAHRSupdateIMU
(double gx, double gy, double gz, double ax, double ay, double az)
{
double recipNorm;
double s0, s1, s2, s3;
double qDot1, qDot2, qDot3, qDot4;
double _2q0, _2q1, _2q2, _2q3, _4q0, _4q1, _4q2 ,_8q1, _8q2;
double q0q0, q1q1, q2q2, q3q3;
// Rate of change of quaternion from gyroscope
qDot1 = 0.5 * (-m_q1 * gx - m_q2 * gy - m_q3 * gz);
qDot2 = 0.5 * ( m_q0 * gx + m_q2 * gz - m_q3 * gy);
qDot3 = 0.5 * ( m_q0 * gy - m_q1 * gz + m_q3 * gx);
qDot4 = 0.5 * ( m_q0 * gz + m_q1 * gy - m_q2 * gx);
// Compute feedback only if accelerometer measurement valid
// (avoids NaN in accelerometer normalisation)
if(!((ax == 0.0) && (ay == 0.0) && (az == 0.0)))
// Fast inverse square-root
// See: http://en.wikipedia.org/wiki/Fast_inverse_square_root
double MadgwickAHRS::invSqrt(double x)
{
/*
float halfx = 0.5f * x;
float y = x;
long i = *(long*)&y;
i = 0x5f3759df - (i>>1);
y = *(float*)&i;
y = y * (1.5f - (halfx * y * y));
return y;*/
return (1.0/sqrt(x)); //we're not in the 70's
}
// IMU algorithm update
void MadgwickAHRS::
madgwickAHRSupdateIMU
(double gx, double gy, double gz, double ax, double ay, double az)
{
double recipNorm;
double s0, s1, s2, s3;
double qDot1, qDot2, qDot3, qDot4;
double o2q0, o2q1, o2q2, o2q3, o4q0, o4q1, o4q2 ,o8q1, o8q2;
double q0q0, q1q1, q2q2, q3q3;
// Rate of change of quaternion from gyroscope
qDot1 = 0.5 * (-m_q1 * gx - m_q2 * gy - m_q3 * gz);
qDot2 = 0.5 * ( m_q0 * gx + m_q2 * gz - m_q3 * gy);
qDot3 = 0.5 * ( m_q0 * gy - m_q1 * gz + m_q3 * gx);
qDot4 = 0.5 * ( m_q0 * gz + m_q1 * gy - m_q2 * gx);
// Compute feedback only if accelerometer measurement valid
// (avoids NaN in accelerometer normalisation)
if(!((ax == 0.0) && (ay == 0.0) && (az == 0.0)))
{
// Normalise accelerometer measurement
recipNorm = invSqrt(ax * ax + ay * ay + az * az);
......@@ -184,74 +182,73 @@ 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
recipNorm = invSqrt(s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3);
s0 *= recipNorm;
s1 *= recipNorm;
s2 *= recipNorm;
s3 *= recipNorm;
// Apply feedback step
qDot1 -= m_beta * s0;
qDot2 -= m_beta * s1;
qDot3 -= m_beta * s2;
qDot4 -= m_beta * s3;
}
{
// normalise step magnitude
recipNorm = invSqrt(s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3);
s0 *= recipNorm;
s1 *= recipNorm;
s2 *= recipNorm;
s3 *= recipNorm;
// Apply feedback step
qDot1 -= m_beta * s0;
qDot2 -= m_beta * s1;
qDot3 -= m_beta * s2;
qDot4 -= m_beta * s3;
}
}
// Integrate rate of change of quaternion to yield quaternion
m_q0 += qDot1 * (1.0 / m_sampleFreq);
m_q1 += qDot2 * (1.0 / m_sampleFreq);
m_q2 += qDot3 * (1.0 / m_sampleFreq);
m_q3 += qDot4 * (1.0 / m_sampleFreq);
// Normalise quaternion
recipNorm = invSqrt(m_q0 * m_q0 + m_q1 * m_q1 +
m_q2 * m_q2 + m_q3 * m_q3);
m_q0 *= recipNorm;
m_q1 *= recipNorm;
m_q2 *= recipNorm;
m_q3 *= recipNorm;
}
// Integrate rate of change of quaternion to yield quaternion
m_q0 += qDot1 * (1.0 / m_sampleFreq);
m_q1 += qDot2 * (1.0 / m_sampleFreq);
m_q2 += qDot3 * (1.0 / m_sampleFreq);
m_q3 += qDot4 * (1.0 / m_sampleFreq);
// Normalise quaternion
recipNorm = invSqrt(m_q0 * m_q0 + m_q1 * m_q1 +
m_q2 * m_q2 + m_q3 * m_q3);
m_q0 *= recipNorm;
m_q1 *= recipNorm;
m_q2 *= recipNorm;
m_q3 *= recipNorm;
}
/* ------------------------------------------------------------------- */
/* --- ENTITY -------------------------------------------------------- */
/* ------------------------------------------------------------------- */
void MadgwickAHRS::display(std::ostream& os) const
{
os << "MadgwickAHRS "<<getName();
try
/* ------------------------------------------------------------------- */
/* --- ENTITY -------------------------------------------------------- */
/* ------------------------------------------------------------------- */
void MadgwickAHRS::display(std::ostream& os) const
{
os << "MadgwickAHRS "<<getName();
try
{
getProfiler().report_all(3, os);
}
catch (ExceptionSignal e) {}
}
} // namespace talos_balance
catch (ExceptionSignal e) {}
}
} // namespace sot
} // namespace dynamicgraph
......
unitTesting/CMakeLists.txt
View file @ 28178759
......@@ -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,7 +68,8 @@ SET (tests
control/test_control_pd
filters/test_filter_differentiator
filters/test_madgwick_ahrs
signal/test_signal
signal/test_depend
signal/test_ptr
......
unitTesting/filters/test_madgwick_ahrs.cpp 0 → 100644
View file @ 28178759
/*
* 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"