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!