Fix/filter

[grizzi]

The current savitzky golay filtering implementation was buggy. At every new optimization the latest input sequence was happended at the previous one with the consequence that where the last was ending (e.g T = t + horizon) the new one was starting (e.g T = t+1) with a time disconitnuity.

A naive implementation of a receding horizon window is added. This has the functionality to being able to trim it at a specific point in time and then to add measurements from this time point onward.

Another minor change concerns the ros controller interface. The previous setup is the one the should be used in real world applications: policy update and optimization happen in separate threads from the ones where the latest observation is set and the input is queried. This setup is inconvenient to debug the algorithm. Therefore the controller_ros interface has a new init method which does not start automatically the threads and a separate start method which instead does start the threads.

If one does not want to do a multithreaded simulation but follow a sequential approach (better for debugging) such as:

• write observation
• update the new policy (blocking)
• get the new input at current time and state
• step the simulation then init must be called but not start. Instead single methods to do what listed before are provided. This setup is a bit brittle but it is at least something that one can start with.

This optional sequential approach is shown in the panda_mppi/src/node/panda_control.cpp example.

[grizzi]

@etiwal FIY: I have created this PR to inform you about these changes .You might want to merge with master to have these on your branch as well. I will try to keep you updated about these kind of chages. In general before working with the code, keep an eye with the master branch and check if there are updates.

Also, open an issue if there is something not working that you could not easily fix. Or better, if you manage to fix this yourself create a branch from master where you just make the fix and then create a pull request for the single fix with a description as I did here.