# Is a Kalman filter suitable to filter projected points positions, given Euler angles of the capturing device?

My system is the following. I use the camera of a mobile device to track an object. From this tracking, I get four 3D points that I project on the screen, to get four 2D points. These 8 values are kinda noisy, due to the detection, so I want to filter them to make the movement smoother and more realistic. As a second measurement, I use the device's gyroscope output, which provides three Euler angles (i.e. the device attitude). Those are more precise and at greater frequency (up to 100 Hz) than 2D positions (around 20 Hz).

My first attempt was with a simple low-pass filter, but the lag was important, so I now try to use a Kalman filter, hoping it will be able to smooth the positions with little delay. As seen in a previous question, one key point in a Kalman filter is the relation between the measurements and the internal state variables. Here the measurements are both my 8 2D point coordinates and the 3 Euler angles, but I'm not sure about what I should use as internal state variables and how I should connect the Euler angles to the 2D points. Hence the primary question, is a Kalman filter even suitable for this problem? And if yes, how?

• If the whole purpose is to smooth the values with minimum delay, you could try to use a minimum-phase filter if you have not already tried. I would be surprised if kalman filtering can give you better than 'minimum-phase delay'. For linear filters I would expect that a minimum-phase filter gives smallest possible delay. – niaren Sep 26 '11 at 13:36
• @niaren : Thanks for the comment, I will study this as well. – Stéphane Péchard Sep 26 '11 at 14:05
• It's unclear what your measurements are. In the Kalman filter framework, "measurements" refer to the quantities that you actually observe. If you're measuring four 3D points (e.g. by fusing multiple camera images together), then those are your measurements. You also need to decide what state variables you are trying to estimate. Are you attempting to track the 3D object locations over time? If so, those are your state variables. It may be appropriate that the 2D representation can be used just for display and not included as part of your model. Additional details will help suggest an approach. – Jason R Sep 26 '11 at 15:23
• As Jsaon says, what your measurements are is not clear. You say: From this tracking, I get four 3D points that I project on a mobile device screen, to get four 2D points. These 8 values are kinda noisy and then later you say What's available to me is the device's gyroscope output, which provides three Euler angles (i.e. the device attitude).. Which is it? The four 2D points, or the three Euler angles? Or does the processing train go Euler angles -> 3D points -> 2D points ? – Peter K. Sep 26 '11 at 15:29
• I have two sets of measurements actually: the detected points positions from the camera, and the Euler angles, but they are not trivial to relate. Plus I'm only interested in the filtered positions as output. I will edit the question to clarify. – Stéphane Péchard Sep 26 '11 at 15:33

Low Pass Filtering

It would be good to know what you mean by "simple low pass filter".

For example, if your measurements at time $k$ are

$$p_{k} = \left[ \begin{array}{c} x_k \\ y_k \end{array} \right]$$

and your low pass filtered estimates are:

$$p^{\tt LPF}_k = \alpha p^{\tt LPF}_{k-1} + (1-\alpha)p_k$$

then you will have quite a large group delay in the filter of about $1/(1-\alpha)$ (for alpha close to 1).

Modeling the Signal: Simplistic Approach

To use the Kalman filter (or any similar approach), you need to have a model for how your measurements are acquired and updated.

Usually this looks like:

$$p^{\tt TRUE}_{k+1} = {\mathbf A} p^{\tt TRUE}_k + {\mathbf B} \epsilon_k$$ where $\epsilon_k$ is the process (driving) noise, ${\mathbf A}$ is the state transition matrix, and ${\mathbf B}$ is your input matrix.

And then your measured $p_k$ are: $$p_k = {\mathbf C} p^{\tt TRUE}_k + {\mathbf D} \nu_k$$ where $\nu_k$ is the output (measurement) noise, ${\mathbf C}$ is the output matrix, and ${\mathbf D}$ is your measurement noise matrix.

Here, the "state" of the model is chosen as the true positions, and the things you measure are the output.

You can then apply the Kalman filter equations to this to get state estimates $\hat{p^{\tt TRUE}_k }$ of the true position.

However, this approach is simplistic because it does not use any knowledge of how the points might move (nor does it use your 4 points and any knowledge you might have about how they move together).

Modeling the Signal: Starting a Better Approach

This page shows how to set up the problem involving the positions and euler angles. It's doing something different from what you need, but the state is:

$$p^{\tt TRUE}_{k} = \left[ x_k\ y_k\ z_k\ \dot{x}_k\ \dot{y}_k\ \dot{z}_k\ \ddot{x}_k\ \ddot{y}_k\ \ddot{z}_k\ \phi\ \psi\ \theta\ \dot{\phi}\ \dot{\psi}\ \dot{\theta}\ \ddot{\phi}\ \ddot{\psi}\ \ddot{\theta}\ \right]^T$$

and the measurements (output) is

$$p_k = \left[ x_k\ y_k\ z_k\ \phi\ \psi\ \theta\ \right ]^T$$

All the model on that page is really doing is saying: $$x^{\tt TRUE}_k = \sum_{n=0}^k \dot{x}^{\tt TRUE}_n n\Delta t + \frac{1}{2} \sum_{n=0}^k \ddot{x}^{\tt TRUE}_n (n\Delta t)^2$$ (but for each of $x,y,$ and $z$).

This is just the classic "equations of motion". See equation (3) here.

• My low pass filtered estimates were: $$p_k = \alpha p_{k-1} + (\alpha - 1) p_k$$ – Stéphane Péchard Sep 27 '11 at 8:46
• @StéphanePéchard: Oops! Yes, I missed that you usually want a unit-gain-at-dc. Even so, the group delay will still be very large for $\alpha$ close to one, which is probably what was not satisfactory with that approach. – Peter K. Sep 27 '11 at 11:34
• I try to apply the article you linked me to. When a At matrix contains derivative time values like $$\Delta t \qquad ; \qquad \frac{1}{2} (\Delta^2)$$, do I need to compute them myself at each time I update the Kalman measurements? – Stéphane Péchard Sep 27 '11 at 13:17
• @StéphanePéchard: These are not "derivative time values". The $\Delta t$ is just $1/f_s$, the time between sample instants. – Peter K. Sep 27 '11 at 13:20
• You need to know what your sampling rate ($f_s$) is, and then the $\Delta t$ will just be the inverse of that. Unless your sampling rate changes you only need to "compute" $\Delta t$ nd $\frac{1}{2} \Delta t^2$ once as they are constant (provided the sampling rate is constant). – Peter K. Sep 27 '11 at 13:38

Your low pass filter may be like;

$$p_k = \alpha p_{k-1} + (1-\alpha) z_k$$

where $z_k$ is $k$th observed data. $p_k$ is $k$th estimated value.

The LPF can be deformed to next:

$$p_k = p_{k-1} + K (z_k - p_{k-1})$$ where $K = (1-\alpha)$.

This is quite similar to Kalman filter. In Kalman filter, $K$ is Kalman gain and generally variable.