EKF-SLAM what should the observation model be?

Question

I am implementing an EKF algorithm for a drone localization, and while I was defining the observation model I got a bit confused.

This is my situation: I have a drone which is able to give me the landmarks' pose in the drone's reference frame (it is a PoseStamped message). In my implementation I have a state vector which contains not only the drone's pose, but also the landmarks' pose, all of them in the fixed reference frame (the world).

Now my question: what should observation model be in this case? I have this question because the landmarks' pose are given, so should the observation model project the estimated landmark pose to the drone's reference frame and in the innovation compute the error between the observed and the estimated poses?

To be a bit more clear:

• My state vector is $ \hat\mu = \begin{bmatrix} x_r & y_r & z_r & \phi_r & \theta_r & \psi_r & x_{m_i} & y_{m_i} & z_{m_i} & \phi_{m_i} & \theta_{m_i} & \psi_{m_i} \end{bmatrix} $ where elements with $r$ represent the pose of the drone and $m_i$ the pose for landmark $i$, everything expressed in the fixed reference frame.
• The observation at time $t$ is $z_t = \begin{bmatrix}x_{m_i}^L & y_{m_i}^L & z_{m_i}^L & \phi_{m_i}^L & \theta_{m_i}^L & \psi_{m_i}^L \end{bmatrix}$ in the drone's reference frame.

So, should the observation model produce the following $h(\hat\mu) = \begin{bmatrix}\hat{x}_{m_i}^L & \hat{y}_{m_i}^L & \hat{z}_{m_i}^L & \hat\phi_{m_i}^L & \hat\theta_{m_i}^L & \hat\psi_{m_i}^L \end{bmatrix}$ ?

Is my reasoning correct? Am I missing something?

Thanks in advance!

Answer 1

Since your state includes the position and orientation of the robot and the landmarks in the global frame, but your measurements are in the robot frame, like you said, the observation model will basically transform the pose estimate from the prediction step into the robot frame followed by addition of noise. Then you compute the innovation by finding the error between the actual measurement and that obtained from the measurement model (both of which are in the robot frame).