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:

position and yaw

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!


1 Answer 1


Ok, just to answer this and close the question. I asked the lab people to test my node with prediction only, and it turned out that it is ok.. So my conclusion is that for some reason, the simulation in my computer is giving me wrong velocities (I don't know why exactly).

My solution to this, was to record a bag in the lab's computer, and use that to continue the development.


Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.

Not the answer you're looking for? Browse other questions tagged or ask your own question.