Rotation composition When using Kalman Filter

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:

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.

• 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? – holmeski Mar 6 '18 at 19:01
• 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. – ejalaa12 Mar 6 '18 at 19:14
• Gotcha, I assumed you were getting feature points or some other product of the state, not the state itself. – holmeski Mar 6 '18 at 19:20
• Ah yes no problem :) I use the Aruco library which can directly give me the pose of the marker in the camera frame – ejalaa12 Mar 6 '18 at 19:26
• Have I not answered your question? – holmeski Mar 14 '18 at 22:07

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.

• 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. – ejalaa12 Mar 6 '18 at 19:24
• I think starting in the static camera frame with a random walk model is a good place to start. – holmeski Mar 6 '18 at 19:28
• 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? – ejalaa12 Mar 6 '18 at 20:55
• 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. – holmeski Mar 6 '18 at 21:28
• Yeah I've done it already. I was asking for advice in the case where the camera moves around. :) – ejalaa12 Mar 6 '18 at 22:09