iCub-main
velocityObserver

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

Parameters

–name name

–lenVel N

–lenAcc N

–thrVel D

–thrAcc D

Ports Accessed

The port the service is listening to.

Ports Created

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:

velocityObserver --name /jointVel --lenVel 20 --thrVel 2.0

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:

yarp connect /icub/right_arm/state:o /jointVel/pos:i
Copyright (C) 2008 RobotCub Consortium.
Author
Ugo Pattacini