Hi guys, I know there are many topics about this theme, but I’m getting mad with this …

I have the **UTM coordinates and orientation** of my state, at the same time, I have **coordinates of the objects** that surround me, **referred to my reference system** and it arrives in a **message of other coordinates referred to the same reference system (mine)**.

**I want to find which of the objects that surround me are closer to those coordinates that reach me.**

For this, what I have been trying to do is put everything in the reference system of the UTM coordinates and then calculate the distance between each object and the message coordinates, for this, I have applied these formulas:

```
posXtransformed = xOrigin*cos(angle) - yOrigin*sin(angle)) + xUTM;
posYtransformed = xOrigin*sin(angle) + yOrigin*cos(angle)) + yUTM;
```

Once calculated, I calculate the distances with dist (), I run it at a low frequency and it works so or so well, but I need it in real time. Here the problems come, I do not know very well why it is due, but doing tests such as changing the sign of the angle of rotation, it works the same (this I think makes no sense, and that is why I wanted to ask to see if the part of the rotation I am doing right or wrong); and during execution (in real time) sometimes it finds the position well, but then when I rotate my reference system, the object it detects also changes …

Any kind of help is welcome, and sorry for the English, I was having a hard time thinking about how to write the problem and I used the google translator.