search

robotology-legacy / codyco-modules

Whole-body Compliant Dynamical Contacts in Cognitive Humanoids
www.codyco.eu
Other
18 stars 13 forks source link

Noise filtering and numerical derivatives #11

Closed andreadelprete closed 7 years ago

andreadelprete commented 10 years ago

Filtering noise in sensor outputs and computing numerical derivatives are fundamental steps in the implementation of most controllers. Currently I'm using 1st order IIR low pass filters for noise filtering (see FirstOrderLowPassFilter) and adaptive windows polynomial fitting for computing numerical derivatives (see adaptWinPolyEstimator). Both are implemented in the C++ library ctrlLib, which is part of the iCub repository.

Current Issues

The reasons why I am not happy with the current solution are:

I would like to replace these classes with more advanced filter implementations, with particular focus on computation time (since these filters should be used inside real-time control loops). Rather than implementing a digital signal processing library from scratch I'd like to use an existing library. Although clearly thought for audio filtering, the C++ library DSPFilters seems particularly interesting, because of the following features:

My intention is to test this library and, if the results are satisfactory, start using it in the iCubWholeBodyState class.

Note on numerical derivatives

The library provides no direct method for numerical derivative computation, but once the input signal has been filtered to attenuate noise, numerical derivatives can be safely computed through exact polynomial fitting (e.g. fitting a 3rd order polynomial to a 4-point moving window).

Other references

Just for the records, I report here other interesting libraries I found in my research.

andreadelprete commented 10 years ago

Any comment is greatly appreciated @traversaro @iron76 @DanielePucci @pattacini @randaz81 @lornat75

pattacini commented 10 years ago

I would consider two main points: (1) filter design, (2) filter implementation.

(1) If the context is to design a filter off-line, knowing the characteristics of the signal to smooth out, there are many solutions off-the-shelf and I would personally rely on MATLAB, namely the fdatool of the Signal Processing Toolbox; it does perform very well. Pro's: very quick learning curve, very complete and automatic, it's pretty likely it outperforms any other similar tool. Con's: you must own the license for the toolbox. On the other hand, If you need others in the project, who are not provided with MATLAB, to design filters, and also you might want to test some fancy filter on-line tuning, then it gets worth depending on a C++ library.

(2) Once designed, it comes out that either a FIR or a IIR filter (e.g. Butterworth, Chebyshev, Elliptic ...) is of very easy implementation, no need to rely on an external library to do that. The performance issue of the present classes in ctrlLib are due to the yarp::sig::Vector wrapper (to make them abstract and very general), but for fast implementations one can think of coding dedicated slim classes explicitly.

Introducing a new dependency always tends to be problematic and, in this scenario, I honestly don't think it will make the difference since the real problem is (1) and not (2), in my view.

Finally, another concern I'd like to highlight: you are applying a two-stage filtering to the signal in order to come up with its derivative: a low-pass filtering followed by the polynomial fitting. What's the overall latency? Are you sure that for the dynamics computation it wouldn't be better to get sooner the derivative even if noisy than having a neater signal but delayed, sometimes significantly?

By the way, @andreadelprete thanks a lot for the paper: it gives a strong mathematical background that was hidden in the article from which I derived the adaptive windows polynomial fitting.

iron76 commented 10 years ago

Dear all, let me first say that I like a lot the discussion. I subscribe what has been written so far by Ugo and Andrea. In my personal (limited) understanding of the problem I do not have yet clear in mind how we want to filter out the signals. In particular, as Ugo pointed out, filtering is performed "knowing the characteristics of the signal to smooth out". Questions are:

  1. What characteristics are we talking about? Examples are: bandwidth (->Butterworth, Chebyshev, Elliptic, Bessel and Legendre filters) and dynamics (->Kalman). Can you give more examples? My point is that in order to filter a signal we need some knowledge about the signal itself and this knowledge is used to compute derivatives. What knowledge do we want to use?
  2. Once we decided what knowledge to use in order to filter out the signal a proper tool for synthesizing the filter is required. As I pointed out different knowledge calls for different tools: bandwidth calls for classical filter design, dynamics call for Kalman filtering.

Cheers, Francesco

P.s. as you know I am working on the whole-body filter which will be definitively not as fast as a simple IIR filter.

Francesco Nori Robotics Brain Cognitive Science Department Cognitive Humanoids Lab Istituto Italiano di Tecnologia Via Morego, 30 16163 Genova, Italy http://people.liralab.it/iron/ francesco.nori@iit.itmailto:francesco.nori@iit.it

Phone: (+39) 010 71 781 420 Fax: +39 010 71 70 817

On Jan 2, 2014, at 12:30 AM, Ugo Pattacini wrote:

I would consider two main points: (1) filter design, (2) filter implementation.

(1) If the context is to design a filter off-line, knowing the characteristics of the signal to smooth out, there are many solutions off-the-shelf and I would personally rely on MATLAB, namely the fdatool of the Signal Processing Toolbox; it does perform very well. Pro's: very quick learning curve, very complete and automatic, it's pretty likely it outperforms any other similar tool. Con's: you must own the license for the toolbox. On the other hand, If you need others in the project, who are not provided with MATLAB, to design filters, and also you might want to test some fancy filter on-line tuning, then it gets worth depending on a C++ library.

(2) Once designed, it comes out that either a FIR or a IIR filter is of very easy implementation, no need to rely on an external library to do that. The performance issue of the present classes in ctrlLib are due to the yarp::os::Vector wrapper (to make them abstract and very general), but for fast implementations one can think of coding dedicated slim classes explicitly.

Introducing a new dependency always tends to be problematic and, in this scenario, I honestly don't think it will make the difference since the real problem is (1) and not (2), in my view.

Finally, another concern I'd like to highlight: you are applying a two-stage filtering to the signal in order to come up with its derivative: a low-pass filtering followed by the polynomial fitting. What's the overall latency? Are you sure that for the dynamics computation it wouldn't be better to get sooner the derivative even if noisy than having a neater signal but delayed, sometimes significantly?

— Reply to this email directly or view it on GitHubhttps://github.com/robotology/codyco/issues/11#issuecomment-31432772.

andreadelprete commented 10 years ago

Reply to @pattacini

I agree with what you said, but:

  1. I'd rather depend on a free open-source light C++ library than on Matlab
  2. I like the possibility to tune filters online, e.g. changing the filter order to find the right tradeoff between noise attenuation and delay, by monitoring the performance of the controller

Regarding the numerical derivatives, delays worry me as well. I found this page on numerical derivatives quite interesting, but it does not talk much about the delay problem. Anyway, my opinion at this moment is that, once you have removed the noise, if you want a "fast" first derivative you can even compute it as (x[n]-x[n-1])/dt (i.e. fitting a line on a 2-point window). This should introduce very little delay. If you want to estimate also the 2nd derivative, then you can fit a parabola on a 3-point window and that's it. I think most of the delay will come from the lowpass filtering and not from the polynomial fitting, but correct me if I'm wrong.

Reply to @iron76

At this moment I was mainly thinking to the case in which we know the frequency characteristics of the signals. In particular, we could collect data from all the sensors, analyze their frequency characteristics and design lowpass filters for noise attenuation. These filters give us noise-free measures, which could be integrated with other information on the signal (e.g. desired trajectories tracked by the controller) by using a Kalman filter (I know that the lowpass filtering could also be included in the Kalman filter, but I preferred to keep these two steps distinct).

traversaro commented 10 years ago

Some random comments:

This DSPFilters seems really cool, I searched for something similar while in Japan but I couldn't find something satisfying like this library. Unfortunately it seems not to be supported a lot upstream (check all the issues without any answer: https://github.com/vinniefalco/DSPFilters/issues or pull request fixing bugs not merged: https://github.com/vinniefalco/DSPFilters/pulls), and the last commits are from two years ago. While we can still use it, the risk of encountering a bug that we cannot solve because we don't have the expertise to modify the librarymshould be considered (an appropriate build infrastructure is also missing).

Just to clarify: we are speaking only about causal numerical derivatives (just asking because Savitzky-Golay is a non-causal filter)?

pattacini commented 10 years ago

@andreadelprete When you rely on a simple fixed window polynomial fitting, I agree, there's no latency problem, considering the last 2,3,4 samples, but then you really need a very strong and robust first-stage filtering. My concern raises since you cascade a first-order filter with the adaptive window polynomial fitting, introducing the latter a latency due to its intrinsic adaptive nature. Generally speaking, a first order filter tends to show pure noise-rejection capability in the band-stop region with high-latency. I suspect you already tried with the adaptive fitting alone disliking the results; it's true that it has mediocre attenuation characteristics as described in the paper you mentioned, but its aim is to filter and compute the derivative in one single step. Consider that latency turns to be often a more critical issue than the noise level in a control loop. Latency is hard to be recovered in the downstream blocks and causes severe instability, while it's likely that downstream blocks are somewhat insensitive to high-frequency noise.

@iron76 Yep, a Kalman filter is far better than an frequency filter since it contains the model inside, but then the point becomes how accurate is that model and how cumbersome is to compute it each time step. That's why it is common to always try out frequency filters first to verify whether it is possible to get roughly the same results (in a given working range) with lower costs. However, I reckon CoDyCo has something to do with the dynamics, right? :) hence I think Kalman might be the preferable solution.

DanielePucci commented 10 years ago

Personally speaking, I'm not an expert of filtering techniques. With that said, I agree with Ugo and Fra that a high - performance filter should exploit the a - priori knowledge of the signal, and that these performances respond to specific objectives, such as cutting frequencies, etc.

In the control respect: true, noise is usually less important than latency for stability concerns when the stability margins are relatively high (e. g. the real parts of the eigenvalues of a linear system are negative and relatively large in absolute value). In the contrary case, noise may induce instability as well. Adaptive control techniques for manipulators generally suffer from these problems because of the small stability margins of closed

@andreadelprete https://github.com/andreadelprete When you rely on a simple fixed window polynomial fitting, I agree, there's no latency problem, considering the last 2,3,4 samples, but then you really need a very strong and robust first-stage filtering. My concern raises since you cascade a first-order filter with the adaptive window polynomial fitting, introducing the latter a latency due to its intrinsic adaptive nature. Generally speaking, a first order filter tends to show pure noise-rejection capability in the band-stop region with high-latency. I suspect you already tried with the adaptive fitting alone disliking the results; it's true that it has mediocre rejection as described in the paper you mentioned, but its aim is to filter and compute the derivative in one single step. Consider that latency turns to be often a more critical issue than the noise level in a control loop. Latency is hard to be recovered in the downstream blocks and causes severe instability, while it's likely that downstream blocks are insensitive to high-frequency noise.

@iron76 https://github.com/iron76 Yep, a Kalman filter is far better than an frequency filter since it contains the model inside, but then the point becomes how accurate is that model and how cumbersome is to compute it each time step. That's why it is common to always try out frequency filters first to verify whether it is possible to get roughly the same results (in a given working range) with lower costs. However, I reckon CoDyCo has something to do with the dynamics, right? :) hence I think Kalman might be the preferable solution.

— Reply to this email directly or view it on GitHubhttps://github.com/robotology/codyco/issues/11#issuecomment-31447006 .

pattacini commented 10 years ago

@traversaro In my understanding, after the reading of the paper, SG technique provides the framework to retrieve useful properties of polynomial fitting based filtering in the frequency domain (i.e. attenuation level, cut-off point ...). The adaptive window polynomial fitting is indeed causal. Is there a point where it is stated that the SG is non-causal?

traversaro commented 10 years ago

@pattacini You are right, my misunderstanding was induced by the fact that I have always seen the fixed-size SG filter used in practice only for time series smoothing (i.e. non-causal filtering).

andreadelprete commented 10 years ago

@traversaro You're right, I saw the unanswered issues and that's a concern. However, if we don't like the software infrastructure, we can always write a new library starting from that code. The most difficult part is filter design, and that can be just copy-pasted. Causal filters are the main interest here (I guess), but we can always create a noncausal filter with zero phase (no delay) starting from a noncausal filter by using the same approach of the "filtfilt" Matlab function or other similar techniques (see this and this for further information).

@pattacini I'm sorry Ugo, I haven't been clear. I haven't cascaded a first-order filter with the adaptive window polynomial fitting, but I used them in different contexts (i.e. 1st order low-pass for force/torque sensors and polynomial fitting for encoders). Using them in cascade would be redundant. Regarding Kalman, I think we don't have to choose between Kalman and frequency domain filtering, because we can use both when we have both a model and the frequency domain characteristics of the sensor. Before "merging" the model predictions with the sensor measurements, we can filter the sensor measurements with a classic digital filter, can't we?

@DanielePucci Latency is for sure critical in control loops, and that's why I like the idea of tuning the filters online to find the right tradeoff between noise attenuation and delay while monitoring the controller performances.

pattacini commented 10 years ago

@andreadelprete Ok, perfect! The cascade existed only in mind then! Much better!

Some notes on mixing Kalman and filtering.

Kalman is equipped with the notion of Gaussian noise on the measurement: it's anyway an opportunity you can explore. Well, we know that noise is not Gaussian, so we need better filters, but we know also that the system is not linear, hence it might be that you're gonna go for more sophisticated Kalman-based observers (EKF, UKF, Particle Filtering ...) that will let you better blend the Kalman and filtering contributions. However, in the limit of linearity, it would be interesting comparing the pure Kalman with a Kalman+filter solution.

Also here the latency introduced by the frequency filter may play a significant role: be careful. In this respect, a viable approach might be to join state and time-delay measurement estimation using Kalman: see this paper.

lornat75 commented 10 years ago

My contribution to the discussion.

I agree with @pattacini we should separate the implementation from the design of the filters/estimators. The library should give the user the freedom to add new estimators/filters (as it probably already does) but for sure other tools (like matlab or other open source libraries) are more suitable for the design.

Online tuning sounds great but I wonder if this shouldn’t be a tool provided separately (maybe as a separate library with its own dependencies).

andreadelprete commented 10 years ago

@pattacini I don't know if everybody agrees with me, but I see Kalman as a "sensor fusion" tool, more than a filter. Kalman not only assumes that noise is Gaussian but also that it is IID, hence it assumes no relationship between the noise at time t and the noise at time t+1. This is quite orthogonal to the approach of classic digital filters. I cannot access the paper you linked because I'm no longer an IIT fellow! :(

@lornat75 Sure, computation and design are two different problems. I haven't checked yet whether the DSPFilter library allows the user to implement a filter that has been externally designed. I agree that this is an important feature and in case it is missing I'd like to add it somehow. Design and online tuning are basically the same thing when it comes to implementation, so if the library provides ways to design filters, it can also be used for online tuning. Separating this into 2 library (say 1 for filter computation, 1 for filter design) could make sense if we consider the dependency on DSPFilter somewhat "heavy". At the moment I cannot tell whether DSPFilter would be a heavy dependency. Maybe after using it for a while I'll have an opinion on this regard.

lornat75 commented 10 years ago

Let me clarify. Are we talking of 1) implementing a generic library (e.g. based on DSPFilter) that provides filters and online tuning algorithms or 2) adding online tuning and filters inside wholeBodyInterface?

pattacini commented 10 years ago

@andreadelprete Here's an alternative link to that paper, which will be active for a while :-)

Well, Kalman is used for filtering too, since the knowledge of the model helps clean the state variables from the noise: as mentioned by @iron76, this is more a dynamics oriented approach. Then, of course, you could use Kalman for sensors fusion, but Kalman observer was not designed originally for that purpose: it is a state estimator whose fortune stems from the countless applications it can be employed for. To any rate, I was referring to the scenario where Kalman might be a viable solution to get the derivative.

It's not clear to me why digital filtering should apply specifically in contexts where the i.i.d. property of the noise is not guaranteed. In my experience that's not true, but I might be missing what you meant.

andreadelprete commented 10 years ago

@lornat75 I was thinking of 1, that is providing users with a digital filter library, which we could either implement ourself or take an existing one (e.g. DPSFilter). Then we can use this library inside the wholeBodyInterface implementation.

@pattacini You're right, Kalman was designed for the case of "sensor measurement" + "model based prediction". Digital filters can also be applied in case of iid gaussian noise, but in this case the noise would cover all the frequency spectrum (if i'm not wrong) with equal magnitude, so it would not be possible to filter it our completely, because you could not separate noise and signal in the frequency domain. Still you can use a filter to attenuate the part of the noise that is in a frequency range where you know the signal is not. For this reason I said that Kalman's approach and classic filter's approach are quite orthogonal, and they could be used together: filters can attenuate high frequency noise, while Kalman can merge the filtered sensor measurements with the model based prediction.

pattacini commented 10 years ago

@andreadelprete Got it! You're provided with many ingredients to cook your recipe and possibly make up new dishes. Be a great chef! :)

lornat75 commented 10 years ago

@andreadelprete

Ok then my comment mostly apply to wholeBodyInterface.

I don't know if you are planning to actually make a wrapper for DSPFilter, but if this is the case maybe it is first better to use DSPFilter directly in your controllers/estimators and see if it is really worth writing a wrapper for users.

DSPFilter looks great: since it uses templates check which compilers are supported.

andreadelprete commented 10 years ago

Will do, thanks to everybody for the nice discussion!

traversaro commented 7 years ago

In a sense, this long discussion is now being solved in https://github.com/robotology/WB-Toolbox/issues/46 , by moving all the filtering out of the interface rather than hidden in the interface. This decision was taken because most of the user of WBI and WB-Toolbox were not aware of the existence of this filters, as they were hidden inside they interface.