Estimates the first and second derivatives of incoming data vector through a least-squares algorithm based on an adpative window.
Collaboration diagram for velocityObserver:
Estimates the first and second derivatives of incoming data vector through a least-squares algorithm based on an adpative window.
Copyright (C) 2010 RobotCub Consortium
Author: Ugo Pattacini
Date: first release 24/10/2008
CopyPolicy: Released under the terms of the GNU GPL v2.0.
Description
This module computes the first derivative (velocity) and the second derivative (acceleration) of vector acquired through an input YARP port and provides them to output YARP ports. The estimation is performed relying on a least-squares algorithm (PDF) which finds the best linear or quadratic regressor upon a certain window of the input samples. The window's length is adaptable, i.e. it varies according the smoothness of the input signal. Therefore, the smoother is the input signal the larger will be the window and the delay introduced by the computation will be negligible. For fast change in the input signal, the window's length is reduced in order to best capture the derivative and to limit the latency.
Note that no knowledge of the sampling time is required since to perform the computation the module relies on the time information stored by the sender within the envelope of the message. If the envelope is not available, then the Time Stamp is the reference time of the machine where the module is running (i.e. the arrival time): of course in this case the computation will be much less precise due to the variable YARP communication latencies.
The figure below shows a snapshot of the estimated velocity and acceleration along with a comparison between the proposed algorithm and a simple moving average filter:
Libraries
- YARP libraries.
- ctrlLib library.
Parameters
–name name
- The parameter name identifies the module's name; all the open ports will be tagged with the prefix <name>/. If not specified /velObs is assumed.
–lenVel N
- The parameter N identifies the maximum window length for buffering the input data (used for the first derivative estimation). Therefore, the window on which the least-squares algorithm operates on will automatically adapts its length from a minimum of 2 to a maximum of N samples. The greater is N, the more filtered is the derivative.
–lenAcc N
- The same as above but for the second derivative's estimation.
–thrVel D
- With this option it is possibile to specify the maximum deviation threshold for the algorithm (used for the first derivative estimation). During the validation phase, indeed, the window's length is considered valid if all the computed values from the linear regressor lie within this threshold wrt their corresponding sample values. If not the window's length is reduced by one step. Each instance of the computation a trial on a window's length enlarged by one step wrt to the current one is performed. Therefore, iteration by iteration, the window's length can be adapted by only one step change.
–thrAcc D
- The same as above but for the second derivative's estimation.
Ports Accessed
The port the service is listening to.
Ports Created
- <name>/pos:i (e.g. /velObs/pos:i) receives the input data vector.
- <name>/vel:o (e.g. /velObs/vel:o) provides the estimated first derivatives.
- <name>/acc:o (e.g. /velObs/acc:o) provides the estimated second derivatives.
- <name>/rpc remote procedure call port useful to shut down the module remotely by sending to this port the 'quit' command.
Input Data Files
None.
Output Data Files
None.
Configuration Files
None.
Tested OS
Linux and Windows.
Example Instantiation of the Module
By launching the following command:
the module will create the listening port /jointVel/pos:i for the acquisition of data vector coming for instance from one of the icub ports. At the same time it will provide the estimated derivatives to /jointVel/vel:o /jointVel/acc:o ports. Here a value of 20 samples is chosen for the velocity maximum window's length and the velocity maximum permitted tolerance is 2.0; for the acceleration default values are used (use –help option to see).
Try now the following:
