Skip to content

GitLab

Commit c79db03c authored by Olivier Stasse's avatar Olivier Stasse
Browse files

[filters] 80 columns policy for filter-differentiator

parent 243c93a0
Hide whitespace changes
Inline Side-by-side
include/sot/core/madgwickahrs.hh 0 → 100644
View file @ c79db03c
//=====================================================================================================
//
// Implementation of Madgwick's IMU and AHRS algorithms.
// See: http://www.x-io.co.uk/node/8#open_source_ahrs_and_imu_algorithms
//
// Date Author Notes
// 29/09/2011 SOH Madgwick Initial release
// 02/10/2011 SOH Madgwick Optimised for reduced CPU load
// 11/05/2017 T Flayols Make it a dynamic graph entity
// 26/03/2019 G Buondonno Converted to double
//
//=====================================================================================================
/*
* Copyright 2017, Thomas Flayols, LAAS-CNRS
*
* This file is part of sot-torque-control.
* sot-torque-control is free software: you can redistribute it and/or
* modify it under the terms of the GNU Lesser General Public License
* as published by the Free Software Foundation, either version 3 of
* the License, or (at your option) any later version.
* sot-torque-control is distributed in the hope that it will be
* useful, but WITHOUT ANY WARRANTY; without even the implied warranty
* of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU Lesser General Public License for more details. You should
* have received a copy of the GNU Lesser General Public License along
* with sot-torque-control. If not, see <http://www.gnu.org/licenses/>.
*/
#ifndef __sot_torque_control_madgwickahrs_H__
#define __sot_torque_control_madgwickahrs_H__
/* --------------------------------------------------------------------- */
/* --- API ------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
#if defined (WIN32)
# if defined (madgwickahrs_EXPORTS)
# define SOTMADGWICKAHRS_EXPORT __declspec(dllexport)
# else
# define SOTMADGWICKAHRS_EXPORT __declspec(dllimport)
# endif
#else
# define SOTMADGWICKAHRS_EXPORT
#endif
/* --------------------------------------------------------------------- */
/* --- INCLUDE --------------------------------------------------------- */
/* --------------------------------------------------------------------- */
#include <dynamic-graph/signal-helper.h>
#include <sot/core/matrix-geometry.hh>
#include <map>
#include "boost/assign.hpp"
#define betaDef 0.01 // 2 * proportional g
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
/* --- 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
} // namespace sot
} // namespace dynamicgraph
#endif // #ifndef __sot_torque_control_madgwickahrs_H__
src/filters/filter-differentiator.cpp
View file @ c79db03c
......@@ -22,7 +22,7 @@
LogFile.close();}
#include <sot/talos_balance/filter-differentiator.hh>
#include <sot/core/filter-differentiator.hh>
#include <sot/core/debug.hh>
#include <dynamic-graph/factory.h>
#include <dynamic-graph/all-commands.h>
......@@ -49,52 +49,70 @@ namespace dynamicgraph
/// so that it can be used by the macros DEFINE_SIGNAL_**_FUNCTION.
typedef FilterDifferentiator EntityClassName;
/* --- DG FACTORY ------------------------------------------------------- */
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(FilterDifferentiator,"FilterDifferentiator");
/* --- DG FACTORY --------------------------------------------------- */
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN
(FilterDifferentiator,"FilterDifferentiator");
/* --- CONSTRUCTION ----------------------------------------------------- */
/* --- CONSTRUCTION ----------------------------------------------------- */
/* --- CONSTRUCTION ----------------------------------------------------- */
/* --- CONSTRUCTION ------------------------------------------------- */
/* --- CONSTRUCTION ------------------------------------------------- */
/* --- CONSTRUCTION ------------------------------------------------- */
FilterDifferentiator::
FilterDifferentiator( const std::string & name )
: Entity(name),
CONSTRUCT_SIGNAL_IN(x, dynamicgraph::Vector)
,CONSTRUCT_SIGNAL_OUT(x_filtered, dynamicgraph::Vector, m_x_dx_ddxSINNER)
,CONSTRUCT_SIGNAL_OUT(dx, dynamicgraph::Vector, m_x_dx_ddxSINNER)
,CONSTRUCT_SIGNAL_OUT(ddx, dynamicgraph::Vector, m_x_dx_ddxSINNER)
,CONSTRUCT_SIGNAL_INNER(x_dx_ddx, dynamicgraph::Vector, m_xSIN)
CONSTRUCT_SIGNAL_IN
(x,
dynamicgraph::Vector)
,CONSTRUCT_SIGNAL_OUT
(x_filtered,
dynamicgraph::Vector, m_x_dx_ddxSINNER)
,CONSTRUCT_SIGNAL_OUT
(dx,
dynamicgraph::Vector, m_x_dx_ddxSINNER)
,CONSTRUCT_SIGNAL_OUT
(ddx,
dynamicgraph::Vector, m_x_dx_ddxSINNER)
,CONSTRUCT_SIGNAL_INNER
(x_dx_ddx,
dynamicgraph::Vector, m_xSIN)
{
Entity::signalRegistration( ALL_INPUT_SIGNALS << ALL_OUTPUT_SIGNALS);
/* Commands. */
addCommand("getTimestep",
makeDirectGetter(*this,&m_dt,
docDirectGetter("Control timestep [s ]","double")));
makeDirectGetter
(*this,&m_dt,
docDirectGetter("Control timestep [s ]","double")));
addCommand("getSize",
makeDirectGetter(*this,&m_x_size,
docDirectGetter("Size of the x signal","int")));
addCommand("init", makeCommandVoid4(*this, &FilterDifferentiator::init,
docCommandVoid4("Initialize the filter.",
"Control timestep [s].",
"Size of the input signal x",
"Numerator of the filter",
"Denominator of the filter")));
makeDirectGetter
(*this,&m_x_size,
docDirectGetter("Size of the x signal","int")));
addCommand("init",
makeCommandVoid4
(*this, &FilterDifferentiator::init,
docCommandVoid4
("Initialize the filter.",
"Control timestep [s].",
"Size of the input signal x",
"Numerator of the filter",
"Denominator of the filter")));
addCommand("switch_filter",
makeCommandVoid2(*this, &FilterDifferentiator::switch_filter,
docCommandVoid2("Switch Filter.",
"Numerator of the filter",
"Denominator of the filter")));
makeCommandVoid2
(*this, &FilterDifferentiator::switch_filter,
docCommandVoid2
("Switch Filter.",
"Numerator of the filter",
"Denominator of the filter")));
}
/* --- COMMANDS ---------------------------------------------------------- */
/* --- COMMANDS ---------------------------------------------------------- */
/* --- COMMANDS ---------------------------------------------------------- */
void FilterDifferentiator::init(const double &timestep,
const int& xSize,
const Eigen::VectorXd& filter_numerator,
const Eigen::VectorXd& filter_denominator)
/* --- COMMANDS ------------------------------------------------------ */
/* --- COMMANDS ------------------------------------------------------ */
/* --- COMMANDS ------------------------------------------------------ */
void FilterDifferentiator::
init(const double &timestep,
const int& xSize,
const Eigen::VectorXd& filter_numerator,
const Eigen::VectorXd& filter_denominator)
{
m_x_size = xSize;
m_dt = timestep;
......@@ -107,8 +125,10 @@ namespace dynamicgraph
return;
}
void FilterDifferentiator::switch_filter(const Eigen::VectorXd& filter_numerator,
const Eigen::VectorXd& filter_denominator)
void FilterDifferentiator::
switch_filter
(const Eigen::VectorXd& filter_numerator,
const Eigen::VectorXd& filter_denominator)
{
LOG("Filter switched with "<<
"Numerator "<< filter_numerator<<std::endl<<
......@@ -118,9 +138,9 @@ namespace dynamicgraph
}
/* --- SIGNALS ---------------------------------------------------------- */
/* --- SIGNALS ---------------------------------------------------------- */
/* --- SIGNALS ---------------------------------------------------------- */
/* --- SIGNALS ------------------------------------------------------ */
/* --- SIGNALS ------------------------------------------------------ */
/* --- SIGNALS ------------------------------------------------------ */
DEFINE_SIGNAL_INNER_FUNCTION(x_dx_ddx, dynamicgraph::Vector)
{
......@@ -135,15 +155,17 @@ namespace dynamicgraph
}
/// ************************************************************************* ///
/// *************************************************************** ///
/// The following signals depend only on other inner signals, so they
/// just need to copy the interested part of the inner signal they depend on.
/// ************************************************************************* ///
/// just need to copy the interested part of the inner signal
/// they depend on.
/// *************************************************************** ///
DEFINE_SIGNAL_OUT_FUNCTION(x_filtered, dynamicgraph::Vector)
{
sotDEBUG(15)<<"Compute x_filtered output signal "<<iter<<std::endl;
sotDEBUG(15)<< "Compute x_filtered output signal "
<< iter<<std::endl;
const dynamicgraph::Vector &x_dx_ddx = m_x_dx_ddxSINNER(iter);
if(s.size()!=m_x_size)
......
src/filters/madgwickahrs.cpp 0 → 100644
View file @ c79db03c
//=====================================================================================================
//
// Implementation of Madgwick's IMU and AHRS algorithms.
// See: http://www.x-io.co.uk/node/8#open_source_ahrs_and_imu_algorithms
//
// Date Author Notes
// 29/09/2011 SOH Madgwick Initial release
// 02/10/2011 SOH Madgwick Optimised for reduced CPU load
// 11/05/2017 T Flayols Make it a dynamic-graph entity
//
//=====================================================================================================
#include <sot/talos_balance/madgwickahrs.hh>
#include <sot/core/debug.hh>
#include <dynamic-graph/factory.h>
#include <dynamic-graph/all-commands.h>
#include <sot/talos_balance/utils/stop-watch.hh>
namespace dynamicgraph
{
namespace sot
{
namespace talos_balance
{
namespace dg = ::dynamicgraph;
using namespace dg;
using namespace dg::command;
using namespace std;
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;
}
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 ------------------------------------------------------- */
/* ------------------------------------------------------------------- */
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);
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 ********************
/* ------------------------------------------------------------------- */
// 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 _2q0, _2q1, _2q2, _2q3, _4q0, _4q1, _4q2 ,_8q1, _8q2, 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);
ax *= recipNorm;
ay *= recipNorm;
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;
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;
if(!((s0 == 0.0) && (s1 == 0.0) && (s2 == 0.0) && (s3 == 0.0)))
{
recipNorm = invSqrt(s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3); // normalise step magnitude
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;
}
/* ------------------------------------------------------------------- */
/* --- ENTITY -------------------------------------------------------- */
/* ------------------------------------------------------------------- */
void MadgwickAHRS::display(std::ostream& os) const
{
os << "MadgwickAHRS "<<getName();
try
{
getProfiler().report_all(3, os);
}
catch (ExceptionSignal e) {}
}
} // namespace talos_balance
} // namespace sot
} // namespace dynamicgraph
Markdown is supported
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