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

Question

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?

Answer 1

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.

Answer 2

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.