Robotics Asked by adiego73 on October 3, 2021

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!

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.

Correct answer by adiego73 on October 3, 2021

Get help from others!

Recent Questions

- How can I transform graph image into a tikzpicture LaTeX code?
- How Do I Get The Ifruit App Off Of Gta 5 / Grand Theft Auto 5
- Iv’e designed a space elevator using a series of lasers. do you know anybody i could submit the designs too that could manufacture the concept and put it to use
- Need help finding a book. Female OP protagonist, magic
- Why is the WWF pending games (“Your turn”) area replaced w/ a column of “Bonus & Reward”gift boxes?

Recent Answers

- Joshua Engel on Why fry rice before boiling?
- haakon.io on Why fry rice before boiling?
- Lex on Does Google Analytics track 404 page responses as valid page views?
- Jon Church on Why fry rice before boiling?
- Peter Machado on Why fry rice before boiling?

© 2024 TransWikia.com. All rights reserved. Sites we Love: PCI Database, UKBizDB, Menu Kuliner, Sharing RPP