Add methods to set priors to iWholeBodyStates

[andreadelprete]

Since iWholeBodyStates should define the interface for a generic estimator it should provide users with the possibility to set priors on the estimated quantities.

After discussing with @iron76 and @traversaro two ways to do this emerged. The user can specify either:

1. the system dynamics and the control inputs, OR
2. the priors on the estimated quantities

   Option 1

The first option is "Kalman-style": the estimation process exploits the knowledge of the system dynamics to predict the future state given the current state and the current control inputs. Then this prediction is fused with the sensor measurements to estimate the state. The main problem I see with this pattern is how to allow the user to specify the system dynamics. Function pointer? Implementing an interface? Moreover I wonder if this interface may fit all possible use-cases.

Option 2

The second option requires the user to do more work because the estimator takes directly the predictions (i.e. which acts as priors) of the future state. The final result is the same, but in this case it is not the estimator to compute the system dynamics, but the user. At a first glance I prefer this solution because it seems less constrained and I have the impression that it could fit to any scenario. Another thing to notice is that the predictions of the future state are usually computed anyway by the controller (some of them are the controller inputs: desired forces, position, velocities), so it doesn't cost anything to the controller to give these values to the estimator, whereas it would be redundant if the estimator had to recompute them using the specified system dynamics.

Anyway the topic is still open for discussion, so please tell me what you think.

[traversaro]

Personally I prefer (at least in the initial phase) Option 2. I believe that a given implementation of iWholeBodyStates can implement an Option 1-style pattern having an implementation specific API for specifying the dynamics of the system, that then internally calls the Option 2 methods. This will permit to have a estimator with this feature without adding too many details to the interface.

[francesco-romano]

I think option 2 is more general and allows more flexibility.

On 20/dic/2013, at 10:14, Silvio Traversaro notifications@github.com wrote:

Personally I prefer (at least in the initial phase) Option 2. I believe that a given implementation of iWholeBodyStates can implement an Option 1-style pattern having an implementation specific API for specifying the dynamics of the system, that then internally calls the Option 2 methods. This will permit to have a estimator with this feature without adding too many details to the interface.

— Reply to this email directly or view it on GitHub.

[iron76]

Software-wise the second option is definitively more flexible. As a general comment I would like to understand better what you mean with "the predictions of the future state are usually computed anyway by the controller ". Reactive controllers (system pole allocation) do not use any predictions. Optimal controllers (e.g. MPC) instead definitively use predictions but not in a stochastic sense. Francesco

---

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 Dec 20, 2013, at 10:19 AM, Francesco Romano wrote:

I think option 2 is more general and allows more flexibility.

On 20/dic/2013, at 10:14, Silvio Traversaro notifications@github.com<mailto:notifications@github.com> wrote:

Personally I prefer (at least in the initial phase) Option 2. I believe that a given implementation of iWholeBodyStates can implement an Option 1-style pattern having an implementation specific API for specifying the dynamics of the system, that then internally calls the Option 2 methods. This will permit to have a estimator with this feature without adding too many details to the interface.

— Reply to this email directly or view it on GitHub.

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

[andreadelprete]

I meant that model-based controllers (e.g. inverse dynamics) typically compute desired values for all the physical quantities they are going to affect. For instance when computing inverse dynamics the controller computes the desired accelerations and then the desired torques. These desired accelerations could be used to bias the acceleration estimation. The same is true for contact forces.

Of course there are also model-based controllers that do not do this (e.g. Operational Space by Khatib), but since some of them (e.g. the ones I'm using) do it, why not to take advantage of this?

[iron76]

If I understand you correctly you are suggesting to use desired states as a prior for state estimation. This implies, in my understanding, that you are suggesting desired states as a good guess for future states. Well, even if I agree with you, I'd rather prefer to think that "the controller + the system dynamics" can be used to obtain a good guess for future states. And this is what the Kalman filter is doing: predicting the future state given the current state and the analytical expression of the applied controller. If the controller is formulated in such a way that it can easily predict future states, then obviously this information can be replaced in the state-prediction step of the Kalman filter without effecting the filter itself and without needs of integrating the system dynamics (which might be time consuming). I hope you agree. Francesco

---

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 11:53 AM, Andrea Del Prete wrote:

I meant that model-based controllers (e.g. inverse dynamics) typically compute desired values for all the physical quantities they are going to affect. For instance when computing inverse dynamics the controller computes the desired accelerations and then the desired torques. These desired accelerations could be used to bias the acceleration estimation. The same is true for contact forces.

Of course there are also model-based controllers that do not do this (e.g. Operational Space by Khatib), but since some of them (e.g. the ones I'm using) do it, why not to take advantage of this?

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

[andreadelprete]

I think the key point is that, in the model-based state prediction, you could either use the desired inputs (e.g. desired joint torques) or the measured inputs (e.g. joint torque sensor measurements). I was suggesting to use the desired inputs, which implies that the resulting joint acceleration prediction would be equal to the desired joint accelerations. Maybe you are more oriented towards the second solution?

[iron76]

I am oriented towards a third solution which uses both. If we are referring to the Kalman filter (KF) and derived versions both desired and measured input are used. Measures are used in the update step. Desired in the prediction step. Moreover we are also not considering the fact that a prior is a probability distribution which is not typically represented in a controller (unless it is a stochastic one). This information is typically in the model of the system. Francesco

---

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 3, 2014, at 1:32 AM, Andrea Del Prete wrote:

I think the key point is that, in the model-based state prediction, you could either use the desired inputs (e.g. desired joint torques) or the measured inputs (e.g. joint torque sensor measurements). I was suggesting to use the desired inputs, which implies that the resulting joint acceleration prediction would be equal to the desired joint accelerations. Maybe you are more oriented towards the second solution?

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

[andreadelprete]

Let's make an example, shall we? Consider joint acceleration estimation. We have the multibody mechanical system dynamics: M ddq + h = tau And we have an acceleration measurement (which may come either from an accelerometer or from numerical derivatives of encoders). My idea was to use the desired joint torques tauD in the model equation to predict the joint accelerations: ddq = M^-1(tauD - h) Alternatively, we could use the measured joint torque tauM: ddq = M^-1(tauM - h) What I don't get is how we can use both. You said we could use input measurements in the update step, but how?

Regarding the probability distribution, I think we don't need a stochastic controller to do this, we just need to set the covariance of the model and the covariance of the measurements, that is how much we want to rely on the model prediction w.r.t. the sensor measurements.

[DanielePucci]

Personally, I would rather use measured torques in the acceleration estimations. The main reason why is that under the assumption of perfect modelling and perfect measurements of q qD, the estimated acceleration is equal to the real one. And we now know how to precisely estimate the system's model (Traversaro) and estimate q and qD (measurements from firmware). As a matter of fact,

tauD = tauM + Errors,

and these errors add estimation imperfections that could ruled out a priori. However, this approach rises a certain number of issues. Beside those previously mentioned in the thread, I would like to emphasize causality problems. I still didn't go through the torque measurement method, but if instantaneous relationships occur between tauM and qDD, i.e.

tauM = tauM (qDD),

this does pose a causality problem when evaluating

qDD = M^-1 (tauM(qDD) - h).

In fact, the above equation is, for all intents and purposes, an algebraic loop.

On Jan 3, 2014 12:23 PM, "Andrea Del Prete" notifications@github.com wrote:

Let's make an example, shall we? Consider joint acceleration estimation. We have the multibody mechanical system dynamics: M ddq + h = tau And we have an acceleration measurement (which may come either from an accelerometer or from numerical derivatives of encoders). My idea was to use the desired joint torques tauD in the model equation to predict the joint accelerations: ddq = M^-1(tauD - h) Alternatively, we could use the measured joint torque tauM: ddq = M^-1(tauM - h) What I don't get is how we can use both. You said we could use input measurements in the update step, but how?

Regarding the probability distribution, I think we don't need a stochastic controller to do this, we just need to set the covariance of the model and the covariance of the measurements, that is how much we want to rely on the model prediction w.r.t. the sensor measurements.

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

[iron76]

In you example ddq = M^-1(tauD - h) is the equation that will be used (i.e. integrated) in the prediction step of the Kalman filter. The other equation ddq = M^-1(tauM - h) is the equation that the Kalman filter uses to estimate the state (q, dq) from the measurement (tauM). Of course this requires to write ddq(q, dq) which is obtained from the system forward dynamics.

As I said the Kalman filter uses both and it is therefore optimal with respect to any other approach using only one of the two equations. Francesco

---

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 3, 2014, at 12:23 PM, Andrea Del Prete wrote:

Let's make an example, shall we? Consider joint acceleration estimation. We have the multibody mechanical system dynamics: M ddq + h = tau And we have an acceleration measurement (which may come either from an accelerometer or from numerical derivatives of encoders). My idea was to use the desired joint torques tauD in the model equation to predict the joint accelerations: ddq = M^-1(tauD - h) Alternatively, we could use the measured joint torque tauM: ddq = M^-1(tauM - h) What I don't get is how we can use both. You said we could use input measurements in the update step, but how?

Regarding the probability distribution, I think we don't need a stochastic controller to do this, we just need to set the covariance of the model and the covariance of the measurements, that is how much we want to rely on the model prediction w.r.t. the sensor measurements.

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

[andreadelprete]

@DanielePucci You're right, using tauM seems to be the best choice, but consider that

1. tauM is delayed due to the discrete sampling of the sensor output and the transmission time
2. tauM needs to be filtered to attenuate noise and this introduce further delay

On the other hand tauD has no delay at all, because it is the torque command that has just been sent to the motor control boards. For these reasons, if the low-level torque controller achieves a sufficiently good tracking, I expect to get better results using tauD rather than tauM. Of course we need verify the accuracy of the torque tracking beforehand.

@iron76 Actually I just realized that the joint acceleration estimation does not fit the classic Kalman formalism. In continuous-time Kalman the model equation takes the form: dx = f(x,u), while the measurement equations is: y = g(x,u) The state of a multi-body mechanical system is x = [q dq]. When estimating ddq we are basically estimating dx, which is not the state, but its derivative. Nothing wrong with it, but it is a little bit outside the classic Kalman.

Anyway, your suggestion was to use u=tauD in the model equation, while in the measurement equation y=tauM , or wasn't it?!

[iron76]

Indeed the Kalman filter allows to use both the desired and measured data to obtain the best (in a Bayesian sense) estimation of the system state (the fact that you want an estimation of the state derivative is not a big deal). As a general comment, in my personal opinion the Kalman filter performs better than frequency based filters whenever a good dynamical model of the system is available. A random walk (dx = x + noise) which is often used in this context is not in my opinion a good model of the system. I suspect (but this needs to be checked) that with a random walk model the KF is simply a low-pass filter whose bandwidth is optimally chosen on the basis of the (state and measurement) noise affecting the system. Francesco

---

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 3, 2014, at 10:17 PM, Andrea Del Prete wrote:

@DanielePuccihttps://github.com/DanielePucci You're right, using tauM seems to be the best choice, but consider that

1. tauM is delayed due to the discrete sampling of the sensor output and the transmission time
2. tauM needs to be filtered to attenuate noise and this introduce further delay

On the other hand tauD has no delay at all, because it is the torque command that has just been sent to the motor control boards. For these reasons, if the low-level torque controller achieves a sufficiently good tracking, I expect to get better results using tauD rather than tauM. Of course we need verify the accuracy of the torque tracking beforehand.

@iron76https://github.com/iron76 Actually I just realized that the joint acceleration estimation does not fit the classic Kalman formalism. In continuous-time Kalman the model equation takes the form: dx = f(x,u), while the measurement equations is: y = g(x,u) The state of a multi-body mechanical system is x = [q dq]. When estimating ddq we are basically estimating dx, which is not the state, but its derivative. Nothing wrong with it, but it is a little bit outside the classic Kalman.

Anyway, your suggestion was to use u=tauD in the model equation, while in the measurement equation y=tauM , or wasn't it?!

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

[andreadelprete]

I would not compare KF and frequency filters because they do two different things. KF merges different probability distributions on the state and it applies in case we have multiple sources of information (e.g. measurements + model) which are corrupted by Gaussian IID noise (i.e. white noise, "frequency-wise speaking"). On the other hand, frequency filters apply to a single source of information to attenuate the range of frequencies where noise acts. To do this they also introduce delay.

I argue that:

1. frequency filtering should be used any time signal and noise are separable (even partially) in the frequency domain
2. KF should be used any time there are multiple sources of information regarding a signal

Hence, in many practical cases I would use both in cascade: frequency filtering first, KF after.

[iron76]

There is a huge theoretical understanding behind the frequency analysis of the Kalman filter and its connection to frequency domain filters. These theoretical results (see for example http://web.mit.edu/course/6/6.433/www/) show that the Kalman filter is equivalent to a spectral factorization which gives the so called whitening filter, i.e. the best filter for predicting a signal. I think all these results are fundamental for approaching any estimation problem. During my PhD I was in a research group that had a great experience on these topics. I definitively have nice references but unfortunately as Silvio knows the book I have is in italian and "under construction". Francesco

---

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 5, 2014, at 11:34 AM, Andrea Del Prete wrote:

I would not compare KF and frequency filters because they do two different things. KF merges different probability distributions on the state and it applies in case we have multiple sources of information (e.g. measurements + model) which are corrupted by Gaussian IID noise (i.e. white noise, "frequency-wise speaking"). On the other hand, frequency filters apply to a single source of information to attenuate the range of frequencies where noise acts. To do this they also introduce delay.

I argue that:

1. frequency filtering should be used any time signal and noise are separable (even partially) in the frequency domain
2. KF should be used any time there are multiple sources of information regarding a signal

Hence, in many practical cases I would use both in cascade: frequency filtering first, KF after.

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

[andreadelprete]

I couldn't find (quickly) info regarding whitening filters in the link you posted. Is it the same principle of whitening transformation ?

[iron76]

Yes, it's its extension to stochastic processes. The link you sent consider stochastic vectors instead.

Francesco

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 5, 2014, at 12:32 PM, Andrea Del Prete wrote:

I couldn't find (quickly) info regarding whitening filters in the link you posted. Is it the same principle of whitening transformationhttp://en.wikipedia.org/wiki/Whitening_transformation ?

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

[andreadelprete]

Ok, I found these notes on Kalman that explain a little bit the "whitening filter" interpretation. Anyway, this doesn't change my idea: I think I would apply frequency filtering to the sensor measurements to attenuate high-frequency noise and then, in cascade, use Kalman to merge the filtered sensor measurements with the model prediction. What do you think about this @iron76 ?

[iron76]

This procedure is definitively pragmatic and therefore I am fine with it. Theoretically speaking I suspect that since Kalman is itself a filter, it might be redundant to filter before Kalman filtering. Anyway I understand that this might require a theoretical investigation that is out of the scope in this discussion. Cheers, Francesco

---

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 8, 2014, at 10:19 AM, Andrea Del Prete wrote:

Ok, I found these notes on Kalmanhttp://www.robots.ox.ac.uk/%7Eian/Teaching/Estimation/LectureNotes2.pdf that explain a little bit the "whitening filter" interpretation. Anyway, this doesn't change my idea: I think I would apply frequency filtering to the sensor measurements to attenuate high-frequency noise and then, in cascade, use Kalman to merge the filtered sensor measurements with the model prediction. What do you think about this @iron76https://github.com/iron76 ?

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

[andreadelprete]

Well, yes, pragmatically I agree that it should work. But my point was mainly theoretical: I think that Kalman cannot replace frequency filtering. I know that in some particular cases it is possible to interpret Kalman as a frequency domain filter, as for instance in the case of a "random walk" model Kalman behaves as a low-pass filter. However, this is only because the model has a clear "frequency-domain" behavior (i.e. zero frequency). This is not true in general, and I think that, given an arbitrary frequency-domain filter, it is not generally possible to find a model to use in Kalman to obtain the same filtering action.

[traversaro]

Closing. During the years, our experience seems to indicate that estimation components are better suited to be implemented in C++ with their own non-generic interface, and access to specific estimated quantities can be provided by specialized interfaces, such as publishing the frame in tf for base estimation, and publishing joint torques estimate in the state published by the ControlBoardWrapper .