I am trying to develop a EKF-SLAM algorithm for a drone, and I want to estimate its position in the inertial/world reference frame. I have as control variables the linear and angular velocities in the drone-reference frame. To do so, I am following this paper: http://ingmec.ual.es/~jlblanco/papers/RangeBearingSLAM6D.pdf in which it states that the position update is:
x_ = x_ + delta_t * (vx * T(0,0) + vy * T(0,1) + vz * T(0,2));
y_ = y_ + delta_t * (vx * T(1,0) + vy * T(1,1) + vz * T(1,2));
z_ = z_ + delta_t * (vx * T(2,0) + vy * T(2,1) + vz * T(2,2));
yaw_ = yaw_ + delta_t * omega_z;
Where T is the following transformation matrix:
$$
\begin{bmatrix} cos(yaw) * cos(pitch) & cos(yaw) * sin(pitch) * sin(roll) - sin(yaw) * cos(roll) & cos(yaw) * sin(pitch) * cos(roll) + sin(yaw) * sin(roll) & 0 \\
sin(yaw) * cos(pitch) & sin(yaw) * sin(pitch) * sin(roll) + cos(yaw) * cos(roll) & sin(yaw) * sin(pitch) * cos(roll) - cos(yaw) * sin(roll) & 0 \\
-sin(pitch) & cos(pitch) * sin(roll) & cos(pitch) * cos(roll) & 0 \\
0 & 0 & 0 & 1 \end{bmatrix}
$$
I have triple-check the transformation matrix and it seems to be fine, also the position updates make sense (or I think it does).
The problem comes when I plot the position to compare ground truth with my estimation, and I see the following:
As you can see between t=40 and 60, the ground truth (red) goes down, and my update (blue) goes to the opposite direction.. after this divergence it continues more or less fine, considering that it drifts (which is the expected). That moment in which one line goes up and the other down, is when the drone starts rotating 180 degrees (as you can see in the second plot - green line), that is why I think the problem is in the transformation matrix.
I've also tried to invert the yaw value or transposing T, but the problem gets bigger.
Something that worth mentioning is that this is pure prediction, so no correction is done at this moment.
Can you spot any error in my reasoning? Is there something that I didn't mention that may worth quadruple-check? Or do you think this is more or less expected and I am creating a problem from nothing?
Hope you can help me! Thanks in advance!
