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

[filters] Add test for filter_differentiator + documentation,

parent 611e145c
Hide whitespace changes
Inline Side-by-side
include/sot/core/filter-differentiator.hh
View file @ 07246255
......@@ -44,11 +44,15 @@
namespace dynamicgraph {
namespace sot {
namespace talos_balance {
/**
* This Entity takes as inputs a signal and applies a low pass filter
/** \addtogroup Filters
\section subsec_filterdiff FilterDifferentiator
This Entity takes as inputs a signal and applies a low pass filter (implemented through CasualFilter)
and computes finite difference derivative.
The input signal is provided through m_xSIN (an entity signal).
The filtered signal is given through m_x_filteredSOUT.
The first derivative of the filtered signal is provided with m_dxSOUT.
The second derivative of the filtered signal is provided with m_ddxSOUT.
*/
class SOTFILTERDIFFERENTIATOR_EXPORT FilterDifferentiator
:public ::dynamicgraph::Entity
......@@ -56,23 +60,30 @@ namespace dynamicgraph {
DYNAMIC_GRAPH_ENTITY_DECL();
public: /* --- SIGNALS --- */
/// Input signals
DECLARE_SIGNAL_IN(x, dynamicgraph::Vector);
/// Output signal x_filtered
DECLARE_SIGNAL_OUT(x_filtered, dynamicgraph::Vector);
DECLARE_SIGNAL_OUT(dx, dynamicgraph::Vector);
DECLARE_SIGNAL_OUT(ddx, dynamicgraph::Vector);
/// The following inner signals are used because this entity has some output signals
/// whose related quantities are computed at the same time by the same algorithm
/// To avoid the risk of recomputing the same things twice, we create an inner signal that groups together
/// all the quantities that are computed together. Then the single output signals will depend
/// The following inner signals are used because this entity has
/// some output signals
/// whose related quantities are computed at the same time by the
/// same algorithm
/// To avoid the risk of recomputing the same things twice,
/// we create an inner signal that groups together
/// all the quantities that are computed together.
/// Then the single output signals will depend
/// on this inner signal, which is the one triggering the computations.
/// Inner signals are not exposed, so that nobody can access them.
/// This signal contains the estimated positions, velocities and accelerations.
/// This signal contains the estimated positions, velocities and
/// accelerations.
DECLARE_SIGNAL_INNER(x_dx_ddx, dynamicgraph::Vector);
protected:
double m_dt; /// sampling timestep of the input signal
int m_x_size;
......@@ -86,12 +97,15 @@ namespace dynamicgraph {
FilterDifferentiator( const std::string & name );
/** Initialize the FilterDifferentiator.
* @param timestep Period (in seconds) after which the sensors' data are updated.
* @param timestep Period (in seconds) after which
* the sensors' data are updated.
* @param sigSize Size of the input signal.
* @param delay Delay (in seconds) introduced by the estimation.
* This should be a multiple of timestep.
* @note The estimationDelay is half of the length of the window used for the
* polynomial fitting. The larger the delay, the smoother the estimations.
* @note The estimationDelay is half of the length of the
* window used for the
* polynomial fitting. The larger the delay,
* the smoother the estimations.
*/
void init(const double &timestep,
const int& xSize,
......@@ -109,7 +123,6 @@ namespace dynamicgraph {
}; // class FilterDifferentiator
} // namespace talos_balance
} // namespace sot
} // namespace dynamicgraph
......
src/CMakeLists.txt
View file @ 07246255
......@@ -55,6 +55,9 @@ SET(plugins
feature/feature-posture
feature/visual-point-projecter
filters/filter-differentiator
filters/madgwickahrs
traces/reader
tools/event
......@@ -144,6 +147,10 @@ SET(${LIBRARY_NAME}_SOURCES
matrix/matrix-svd
filters/causal-filter
utils/stop-watch
)
ADD_LIBRARY(${LIBRARY_NAME}
......
src/filters/filter-differentiator.cpp
View file @ 07246255
......@@ -33,8 +33,6 @@ namespace dynamicgraph
{
namespace sot
{
namespace talos_balance
{
#define ALL_INPUT_SIGNALS m_xSIN
......@@ -206,6 +204,5 @@ namespace dynamicgraph
catch (ExceptionSignal e) {}
}
} // namespace talos_balance
} // namespace sot
} // namespace dynamicgraph
src/filters/madgwickahrs.cpp
View file @ 07246255
//=====================================================================================================
//=========================================================================
//
// Implementation of Madgwick's IMU and AHRS algorithms.
// See: http://www.x-io.co.uk/node/8#open_source_ahrs_and_imu_algorithms
......@@ -8,11 +8,11 @@
// 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/madgwickahrs.hh>
#include <sot/core/debug.hh>
#include <dynamic-graph/factory.h>
......@@ -53,8 +53,9 @@ namespace dynamicgraph
: 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)
,CONSTRUCT_SIGNAL_OUT(imu_quat, dynamicgraph::Vector,
m_gyroscopeSIN <<
m_accelerometerSIN)
,m_initSucceeded(false)
,m_beta(betaDef)
,m_q0(1.0)
......@@ -66,16 +67,23 @@ namespace dynamicgraph
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")));
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)
......@@ -101,7 +109,8 @@ namespace dynamicgraph
{
if(!m_initSucceeded)
{
SEND_WARNING_STREAM_MSG("Cannot compute signal imu_quat before initialization!");
SEND_WARNING_STREAM_MSG
("Cannot compute signal imu_quat before initialization!");
return s;
}
const dynamicgraph::Vector& accelerometer = m_accelerometerSIN(iter);
......@@ -110,8 +119,9 @@ namespace dynamicgraph
getProfiler().start(PROFILE_MADGWICKAHRS_COMPUTATION);
{
// Update state with new measurment
madgwickAHRSupdateIMU( gyroscope(0), gyroscope(1), gyroscope(2),
accelerometer(0), accelerometer(1), accelerometer(2));
madgwickAHRSupdateIMU
( gyroscope(0), gyroscope(1), gyroscope(2),
accelerometer(0), accelerometer(1), accelerometer(2));
if(s.size()!=4)
s.resize(4);
s(0) = m_q0;
......@@ -125,7 +135,7 @@ namespace dynamicgraph
}
/* --- COMMANDS ---------------------------------------------------------- */
/* --- COMMANDS ------------------------------------------------------ */
/* ------------------------------------------------------------------- */
// ************************ PROTECTED MEMBER METHODS ********************
......@@ -147,12 +157,15 @@ namespace dynamicgraph
}
// IMU algorithm update
void MadgwickAHRS::madgwickAHRSupdateIMU(double gx, double gy, double gz, double ax, double ay, double az)
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;
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);
......@@ -160,7 +173,8 @@ namespace dynamicgraph
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)
// 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
......@@ -186,12 +200,15 @@ namespace dynamicgraph
// 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;
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
// normalise step magnitude
recipNorm = invSqrt(s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3);
s0 *= recipNorm;
s1 *= recipNorm;
s2 *= recipNorm;
......@@ -212,7 +229,8 @@ namespace dynamicgraph
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);
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;
......
unitTesting/CMakeLists.txt
View file @ 07246255
......@@ -22,82 +22,88 @@ ADD_DEFINITIONS(-DTESTS_DYNLIBSUFFIX="${CMAKE_SHARED_LIBRARY_SUFFIX}")
# the name of the variable for test EXECUTABLE_NAME is
# TEST_${EXECUTABLE_NAME}_LIBS
SET(TEST_tsot_LIBS
task
feature-visual-point
gain-adaptive
)
task
feature-visual-point
gain-adaptive
)
IF(WIN32)
SET(TEST_test_traces_EXT_LIBS
${DYNAMIC_GRAPH_PLUGINDIR}/tracer${CMAKE_LINK_LIBRARY_SUFFIX}
)
SET(TEST_test_traces_EXT_LIBS
${DYNAMIC_GRAPH_PLUGINDIR}/tracer${CMAKE_LINK_LIBRARY_SUFFIX}
)
ELSE(WIN32)
SET(TEST_test_traces_EXT_LIBS
${DYNAMIC_GRAPH_PLUGINDIR}/tracer${CMAKE_SHARED_LIBRARY_SUFFIX}
)
SET(TEST_test_traces_EXT_LIBS
${DYNAMIC_GRAPH_PLUGINDIR}/tracer${CMAKE_SHARED_LIBRARY_SUFFIX}
)
ENDIF(WIN32)
SET(TEST_test_gain_LIBS
gain-adaptive feature-visual-point
)
gain-adaptive feature-visual-point
)
SET(TEST_test_task_LIBS
gain-adaptive feature-visual-point task
)
gain-adaptive feature-visual-point task
)
SET(TEST_test_mailbox_LIBS
mailbox-vector
)
mailbox-vector
)
SET(TEST_test_control_pd_LIBS
control-pd
)
SET(TEST_test_filter_differentiator_LIBS
filter-differentiator
)
#test paths and names (without .cpp extension)
SET (tests
dummy
dummy
control/test_control_pd
signal/test_signal
signal/test_depend
signal/test_ptr
signal/test_dep
signal/test_ptrcast
control/test_control_pd
sot/tsot
filters/test_filter_differentiator
traces/files
traces/test_traces
signal/test_signal
signal/test_depend
signal/test_ptr
signal/test_dep
signal/test_ptrcast
task/test_flags
task/test_gain
task/test_multi_bound
task/test_task
sot/tsot
tools/test_boost
tools/test_mailbox
tools/test_matrix
tools/test_robot_utils
math/matrix-twist
math/matrix-homogeneous
traces/files
traces/test_traces
matrix/test_operator
)
task/test_flags
task/test_gain
task/test_multi_bound
task/test_task
tools/test_boost
tools/test_mailbox
tools/test_matrix
tools/test_robot_utils
math/matrix-twist
math/matrix-homogeneous
matrix/test_operator
)
# TODO
IF(WIN32)
LIST(REMOVE_ITEM tests tools/test_mailbox)
LIST(REMOVE_ITEM tests tools/test_mailbox)
ENDIF(WIN32)
IF(UNIX)
ADD_LIBRARY(pluginabstract
SHARED
tools/plugin.cc)
tools/plugin.cc)
ADD_EXECUTABLE(test_abstract_interface
tools/test_abstract_interface.cpp
)
tools/test_abstract_interface.cpp
)
TARGET_LINK_LIBRARIES(test_abstract_interface
pluginabstract ${CMAKE_DL_LIBS} ${Boost_LIBRARIES})
......@@ -112,39 +118,39 @@ ADD_DEFINITIONS(-DDEBUG=2)
INCLUDE_DIRECTORIES(${CMAKE_SOURCE_DIR}/include)
FOREACH(test ${tests})
GET_FILENAME_COMPONENT(EXECUTABLE_NAME ${test} NAME)
GET_FILENAME_COMPONENT(EXECUTABLE_NAME ${test} NAME)
ADD_EXECUTABLE(${EXECUTABLE_NAME} ${test}.cpp)
ADD_EXECUTABLE(${EXECUTABLE_NAME} ${test}.cpp)
TARGET_LINK_LIBRARIES(${EXECUTABLE_NAME} ${PROJECT_NAME})
ADD_DEPENDENCIES (${EXECUTABLE_NAME} ${PROJECT_NAME})
TARGET_LINK_LIBRARIES(${EXECUTABLE_NAME} ${PROJECT_NAME})
ADD_DEPENDENCIES (${EXECUTABLE_NAME} ${PROJECT_NAME})
IF( TEST_${EXECUTABLE_NAME}_LIBS )
TARGET_LINK_LIBRARIES(${EXECUTABLE_NAME} ${TEST_${EXECUTABLE_NAME}_LIBS})
ADD_DEPENDENCIES (${EXECUTABLE_NAME} ${TEST_${EXECUTABLE_NAME}_LIBS})
ENDIF( TEST_${EXECUTABLE_NAME}_LIBS )
IF( TEST_${EXECUTABLE_NAME}_LIBS )
TARGET_LINK_LIBRARIES(${EXECUTABLE_NAME} ${TEST_${EXECUTABLE_NAME}_LIBS})
ADD_DEPENDENCIES (${EXECUTABLE_NAME} ${TEST_${EXECUTABLE_NAME}_LIBS})
ENDIF( TEST_${EXECUTABLE_NAME}_LIBS )
IF( TEST_${EXECUTABLE_NAME}_EXT_LIBS )
TARGET_LINK_LIBRARIES(${EXECUTABLE_NAME} ${TEST_${EXECUTABLE_NAME}_EXT_LIBS})
ENDIF( TEST_${EXECUTABLE_NAME}_EXT_LIBS )
IF( TEST_${EXECUTABLE_NAME}_EXT_LIBS )
TARGET_LINK_LIBRARIES(${EXECUTABLE_NAME} ${TEST_${EXECUTABLE_NAME}_EXT_LIBS})
ENDIF( TEST_${EXECUTABLE_NAME}_EXT_LIBS )
IF (UNIX)
TARGET_LINK_LIBRARIES(${EXECUTABLE_NAME} ${CMAKE_DL_LIBS})
ENDIF(UNIX)
IF (UNIX)
TARGET_LINK_LIBRARIES(${EXECUTABLE_NAME} ${CMAKE_DL_LIBS})
ENDIF(UNIX)
PKG_CONFIG_USE_DEPENDENCY(${EXECUTABLE_NAME} dynamic-graph)
PKG_CONFIG_USE_DEPENDENCY(${EXECUTABLE_NAME} dynamic-graph)
IF(BUILD_PYTHON_INTERFACE)
PKG_CONFIG_USE_DEPENDENCY(${EXECUTABLE_NAME} dynamic-graph-python)
ENDIF(BUILD_PYTHON_INTERFACE)
# Link against Boost.
TARGET_LINK_LIBRARIES(${EXECUTABLE_NAME} ${Boost_LIBRARIES} ${Boost_SYSTEM_LIBRARY})
ADD_TEST(${test} ${EXECUTABLE_NAME})
# Link against Boost.
TARGET_LINK_LIBRARIES(${EXECUTABLE_NAME} ${Boost_LIBRARIES} ${Boost_SYSTEM_LIBRARY})
ADD_TEST(${test} ${EXECUTABLE_NAME})
IF (UNIX)
SET(EXTRA_LD_LIBRARY_PATH $ENV{LD_LIBRARY_PATH})
SET_PROPERTY(TEST ${test} PROPERTY
ENVIRONMENT "LD_LIBRARY_PATH=${CMAKE_BINARY_DIR}/src:${CMAKE_INSTALL_PREFIX}/${CMAKE_INSTALL_LIBDIR}:${BOOST_ROOT}/lib:${EXTRA_LD_LIBRARY_PATH}")
ENDIF(UNIX)
IF (UNIX)
SET(EXTRA_LD_LIBRARY_PATH $ENV{LD_LIBRARY_PATH})
SET_PROPERTY(TEST ${test} PROPERTY
ENVIRONMENT "LD_LIBRARY_PATH=${CMAKE_BINARY_DIR}/src:${CMAKE_INSTALL_PREFIX}/${CMAKE_INSTALL_LIBDIR}:${BOOST_ROOT}/lib:${EXTRA_LD_LIBRARY_PATH}")
ENDIF(UNIX)
ENDFOREACH(test)
unitTesting/filters/test_filter_differentiator.cpp 0 → 100644
View file @ 07246255
/*
* 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/filter-differentiator.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::FilterDifferentiator *aFilterDiff = new
FilterDifferentiator("filter_differentiator");
Eigen::VectorXd filter_num(7),filter_den(7);
filter_num(0) = 2.16439898e-05;
filter_num(1) = 4.43473520e-05;
filter_num(2) = -1.74065002e-05;
filter_num(3) = -8.02197247e-05;
filter_num(4) = -1.74065002e-05;
filter_num(5) = 4.43473520e-05;
filter_num(6) = 2.16439898e-05;
filter_den(0) = 1.;
filter_den(1) = -5.32595322;
filter_den(2) = 11.89749109;
filter_den(3) = -14.26803139;
filter_den(4) = 9.68705647;
filter_den(5) = -3.52968633;
filter_den(6) = 0.53914042;
double timestep=0.001;
int xSize=16;
aFilterDiff->init(timestep,xSize,filter_num,filter_den);
srand(0);
dynamicgraph::Vector aVec(16);
for(unsigned int i=0;i<16;i++)
aVec(i) = (double)(i+rand()%100);
aFilterDiff->m_xSIN = aVec;
aFilterDiff->m_x_filteredSOUT.recompute(0);
output_test_stream output;
ostringstream anoss;
dynamicgraph::Vector outVec;
aFilterDiff->m_x_filteredSOUT.get(output);
BOOST_CHECK(output.is_equal("82.5614\n"
"86.5403\n"
"78.5826\n"
"17.9049\n"
"96.4874\n"
"39.7886\n"
"91.5139\n"
"98.4769\n"
"56.6988\n"
"29.8415\n"
"71.6195\n"
"37.7992\n"
"101.461\n"
"71.6195\n"
"76.5931\n"
"40.7834\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