# How to produce a continuous variation of a discontinuous function?

I have a differential equation that connects the "velocity" of a point in the FOV of a camera with the velocities of a robot's joints, that is $$\dot s=J(s) \dot q$$ where s is a vector with the $x$,$y$ coordinates of the point in the FOV, $J$ is the interaction matrix and $q$ is the vector of the joint positions.

If I have a certain point whose velocity I am tracking and this point remains in the FOV, then $\dot s$ is well defined. But if I change this point online, that is at the time instant $t$ I have point $s_t$ and at the time instant $t+dt$ I have the point $s_{t+dt}$, then $\dot s$ is not defined.

Can I create a filter to produce a continuous variation of $\dot s$? If not, what can I do?

More specifically, I want to perform occlusion avoidance. In order to do this I want to compute the minimum distance of each feature point of my target object from the possibly occluding object. But, obviously, this distance can be discontinuous due to the fact that another possibly occluding object can appear in the FOV nearer to my target than the previously measured.

• Won't a filter cause the estimation of the velocity of your new point of interest to be distorted by the velocity of the old point of interest? If I understand your question, what you really want to do is stop following one point and start following another (and possibly do the stopping and starting as quickly as possible)? Maybe add some info about what is important in stopping and starting? For instance, do you need to bring the point of interest inside some radius from the center of the FOV to insure tracking? Are there limits to allowed robot acceleration? – hauptmech Jun 5 '16 at 11:34
• What do you mean by that it is not defined? Do you mean that the integral does not have a closed form solution in general? – fibonatic Jun 5 '16 at 11:52
• @fibonatic $s$ is not continuous and therefore not differentiable – Controller Jun 5 '16 at 15:36

It happens many times that set-points fed in our systems do change in a step-wise manner. Your intuition of filtering those variations is correct and represents a common practice.

Here I'd give two cases:

1. You have direct access to $\dot{s}$, which is thus your velocity reference varying step-wise. Then, you could consider a simple frequency based filter, which does a pretty good job.
2. You have access to $s$, which is your position set-point, possibly varying step-wise. I therefore assume you're then computing the corresponding velocity $\dot{s}$ by means of differentiation, which is intrinsically an ill-posed method that enhances noise. In this context, my warm advice is to apply state-space filtering to $s$, which not only smooths out step-wise transitions in the input position, but also provides you with robust estimates of the velocity.

The second approach falls within the well known area of Kalman filtering, on which there is a wide availability of material and results in literature. In essence, it's a matter of choosing a template dynamical model underlying your observations $s$ and the filter will give you back the estimated internal state comprising the velocity $\dot{s}$.

Typical selections foresee a model describing a constant speed dynamics with noise on the acceleration, and observed positions. Higher order templates (e.g. constant acceleration with noise on the jerk and so on) are also possible.

Even though your point in the FOV won't move in accordance with the chosen template, the filter will significantly smooth out the velocity during fast transitions, as you desire.

• If I got this correctly, the first approach smooths out the control signal a posteriori. I don't want that. I want to use the smoothed out/filtered value of $s$ in my analysis. Would the second approach help me with that? Could you elaborate some more? – Controller Jun 5 '16 at 15:54
• I've put some more details in the answer. – Ugo Pattacini Jun 5 '16 at 16:28