3
$\begingroup$

I am implementing a Kalman Filter for the following situation. I have a camera set in a room that can detect the position and orientation of a marker (ARUCO) in the room.

Therefore I have the following frame transformations: transformations

What I want to filter with the Kalman is the position and orientation of the marker: $[x, y, z, \phi, \theta, \psi]$ in the room frame.

I already have a prediction model for the marker (constant-velocity).

I am writing the observation model equations. I have the following relationships:

$$ X_{observed} = X_{marker/camera} = R_{camera/room}^T \cdot X_{marker/room}$$ and $$R_{observed} = R_{marker/camera} = R_{camera/room}^T \cdot R_{marker/room}$$

With these expressions, I express the observation with respect to the estimated variables: $$X_{marker/room} = (x,y,z)^T$$ $$R_{marker/room} = eul2mat(\phi, \theta, \psi)$$

However, the function $eul2mat$ and the matrix multiplication introduce some non-linearity. Which forces me to use an Extended Kalman Filter. Now I still can figure out the math of this. But the math becomes too complicated for what I'm trying to solve

Of course, if I look at the problem differently. Let's say I only try to filter the pose of the marker in the camera frame. Then the equations are much simpler (a lot of identity matrices appear), and the system is linear.

So here is my question:
Is there a way to make the equations of this system simpler?

PS : This is a simple case, where I don't really need to estimate the full transform directly (marker to room frame). But there might be cases where I need to estimate the full transform so that the state vector might be available for another filtering.

Improve this question
$\endgroup$
7
  • $\begingroup$ I'm curious where those identify matrices are coming from. Are you assuming there will only be small changes in orientation between the camera and marker? $\endgroup$
    – holmeski
    Commented Mar 6, 2018 at 19:01
  • $\begingroup$ Well, i meant that if I estimate the position of the marker in the camera frame then the observation is equal to the state vector. We are observing directly what we want to estimate. $\endgroup$
    – ejalaa12
    Commented Mar 6, 2018 at 19:14
  • $\begingroup$ Gotcha, I assumed you were getting feature points or some other product of the state, not the state itself. $\endgroup$
    – holmeski
    Commented Mar 6, 2018 at 19:20
  • $\begingroup$ Ah yes no problem :) I use the Aruco library which can directly give me the pose of the marker in the camera frame $\endgroup$
    – ejalaa12
    Commented Mar 6, 2018 at 19:26
  • $\begingroup$ Have I not answered your question? $\endgroup$
    – holmeski
    Commented Mar 14, 2018 at 22:07

1 Answer 1

Reset to default
0
$\begingroup$

Your post makes it seem like you have the camera state. If this is true then you could estimate the pose of the marker in the camera frame. You could transform the marker pose estimates to the room frame outside of the kalman filter.

You also state you're assuming a constant velocity model. This would involve estimating the velocity of the maker in addition to the position. This is missing from your state vector which only includes position and orientation.

You have not mentioned if your camera will be moving around. If the camera does move around this would slightly complicate the prediction update of your kalman filter (assuming you have information about the camera state). You will have to update the predicted marker state given the (assumed) known change in camera state.

Moreover, if you choose to estimate the velocity of the marker with in a moving and rotating camera frame, you will be estimating the velocity in a non inertial reference frame. This can be handled but you may find it easier to estimate the velocity of the marker in the room frame which is inertial.

Improve this answer
$\endgroup$
5
  • $\begingroup$ I agree. If I have the camera state (a fixed camera for example) then I can estimate in the camera frame and then transform to the room frame. Also, yes I forgot to add the speed in the state vector thank you, I'll edit that. Or let's assume the prediction model is a random walk. Anyway, yes the camera will be moving later, that's exactly why I want to estimate the pose of the marker in the room frame which is inertial. For the moment the camera is fixed so I don't estimate it in the state vector. But the idea was to implement it step by step. $\endgroup$
    – ejalaa12
    Commented Mar 6, 2018 at 19:24
  • $\begingroup$ I think starting in the static camera frame with a random walk model is a good place to start. $\endgroup$
    – holmeski
    Commented Mar 6, 2018 at 19:28
  • $\begingroup$ I already have. But if I do the estimation in the room frame, since I estimate the Euler angles. I need to convert my matrix product back to Euler angles. So when doing an extended Kalman filter, I need to derivate the $mat2euler$ and $euler2mat$ functions. I use sympy to obtain these expressions. But they quickly become cumbersome. Have you ever done something similar? If so, how did you do it? Would you recommend using quaternion instead? $\endgroup$
    – ejalaa12
    Commented Mar 6, 2018 at 20:55
  • $\begingroup$ I think you should estimate stuff in the camera frame and convert it to room frame outside of the kalman filter. Quaternions are better but I've never needed them. $\endgroup$
    – holmeski
    Commented Mar 6, 2018 at 21:28
  • $\begingroup$ Yeah I've done it already. I was asking for advice in the case where the camera moves around. :) $\endgroup$
    – ejalaa12
    Commented Mar 6, 2018 at 22:09

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.

Not the answer you're looking for? Browse other questions tagged or ask your own question.