I want to design an EKF to estimate the position of a UAV. If I were doing this with Euler angles then I would have a state vector that would look like \begin{bmatrix}north&east&down&vel_x&vel_y&vel_z&a_x&a_y&a_z&y&p&r& \omega_x&\omega_y&\omega_z\end{bmatrix}
With velocities and accelerations being in body frame and exactly like I obtain from the sensors. These would be connected to the position states via the angles of roll, pitch, yaw.
However, now I have quaternions and I don't know how to form the dynamical system. Based on what I have read the state has to be: \begin{bmatrix}north&east&down&vel_n&vel_e&vel_d&a_n&a_e&a_d&q_1&q_2&q_3&q_4& \omega_x&\omega_y&\omega_z\end{bmatrix}
The relationship between $ \dot{q} $ and $q$ is $ \dot{q} = \frac{S(\omega)}{2} q$, where
$S(\omega) = \begin{bmatrix} 0 & -\omega_x & -\omega_y &-\omega_z\\ \omega_x & 0 & \omega_z &-\omega_y\\ \omega_y & -\omega_z & 0 &\omega_x\\ \omega_z & \omega_y &-\omega_x&0\\ \end{bmatrix}$
So the major difference is that I use the world frame for the velocities and accelerations. I presume this would require a preprocessing step using the orientation of the vehicle to transform the measurements for acceleration from body frame to world frame during the kalman correction step. The Kalman filter is given measurements in world frame which is not exactly what I get from the sensors so I imagine my measurement noise will not be populated using values I get from a spec sheet but I will rather have to measure those. The nonlinearity in the system comes only from the $S(\omega)$ function. Does this sound like I'm on the right track?
If anyone has some tutorial that shows exactly how to do this I would greatly appreciate it.
