Indoor auto navigation

Hi everyone, i am using Pozyx beacon to obtain a local reference system for indoor localization. Even if i am struggling with the starting position of the drone on the MP map (it drifts a bit with respect to the coordinate lat, lng i give in the BCN params), i was able to change the EKF origin and the drone can take off and loiter with good stability. Now i want to provide some waypoints to follow. Any suggestion on how can i do it? Now i am working with an arduino, however i could move to ros using a rapberry board. The idea was to pass in some way the local map waypoints, and convert them into globall NED (which i assume is the main reference used by ardupilot). Any document or suggestion is well accepted

1 Like

The easiest way is to convert Ned into lat lon. And use normal MissionPlanner waypoints with the calculated lat lon coordinates.

Yes, once the vehicle is appearing on the map then the EKF has a good position estimate and a regular mission can be uploaded using MP’s Plan screen (or QGC or other GCSs have similar functions) and then executed in Auto mode.

Copter also supports various real-time controls in Guided mode including attitude, velocity, position (both local and in lat,lon,alt frame). To use Guided mode controls you normally send in the commands using MAVLink which is described here. These controls are also available to Lua scripts running on the autopilot.

Thank you all for the answers. I’ll try both methods. Moreover, i’ve already talked about this problem with lucas, but i wasn’t able to solve it. Maybe @rmackay9 can help me too. As i said when i connect the drone with GCS (MP) it appears on the map at lat and lon given by BCN_params. But after a minute it drifts a bit. Do you think it is due to some KF tuning? or is an error given by a bad spatial configuration of the anchors? I noticed that trying differenct anchors displacements i have different drift, but i haven’t achieve perfect position yet (minimum error around 3 meters). Any suggestions? thank you in advance

I did some tests about this starting position. I noticed that the drift follows the direction given by the BCN_ORIENT_YAW param, thus i think is something correlated to the compass. I did a calibration but nothing changed. Is there a parameter (some offsets or similar) i can try to modify?. After this starting drift the drone moves correctly, but the problem bothers me since i next want to visualize the exact position on the map during flight

00000003.BIN (860 KB)
This is my last indoor flight. I wanted just to do an auto mission with take off, loiter and land, everything at 1m. However the copter had alt problems and then started to move 2 meters of the position and crashed to the indoor cage. I noticed from the log that altitude was disturbed, i don’t know why, the rangefinder works good. I look for some docs and see that OA_type is already zero, and EK3_SRC1_POSZ = 2. Moreover there still is the error with the compass that i cannot overcome

Found out that here

there was the same problem i have, which is the EKF origin setting. Can you confirm me that this problem is not solved yet?

In my setup this problem is still present. As described in my post, what I think is happening is that the origin of the EK3 is set at lat and lon given by BCN_params before starting to the use the beacons for positioning. When the position reported by the beacon is first used the vehicle position instantly jumps a few meters from the EKF3 origin to the actual position reported by the beacon.

I would appreciate it if any of you with a deeper understanding of this could take a look at it.

Could be the problem of the offset in the origin due to this passage?

I think there is something not good with that if and the boolean bcnOriginEstInit. But i am not a very good programmer. If anyone can look at this and tell me something i would appreciate it

Please correct me if i am wrong. I understand, looking at the code, that for what the Pozyx beacons are concerned, origin of the filter is first defined at lat,lon given by the BCN_params. Then during the so called rngBcnAlignment phase the receiverPos is moved to the center of the beacons and, when it is completed, it is moved from the bcnPosOffsetNED coordinates. However, for what i see, during the first centering behaviour the scale is not correct with respect to the beacons configurations, and the drone moves away. Probably this was done for accomplish immediate flight? But delete it and wait for AlignmentCompleted would probably be better. I repeat, i am not expert, so please correct me if i didn’t get it.

I have also been looking at the code and I agree with you that something must be wrong with the rngBcnAlignment but I don’t understand what you mean by “the scale is not correct”. On the other hand I don’t have enough confidence to narrow down the problem as I don’t have that much experience with the ardupilot code.

I had intended to post an issue on github quoting these posts and see if someone with more knowledge could take a look at it.

In any case, have you managed to figure out anything else?

With “the scale is not corrected” i mean that i see the drone moving from BCN_LAT and BCN_LNG (from now on referred to as beacons_origin) to the center of the beacons perimeter. However, if my configuration is a rectangle of (2.5 x 5)meters i see on the MP map the drone moving from the beacons_origin of something like 10meters (should be around 3 meters).

I am not an expert programmer either, but i think the problem is on this difference:
line 72 of AP_NavEKF3_RngBcnFusion.cpp

// predicted range
    Vector3F deltaPosNED = stateStruct.position - rngBcnDataDelayed.beacon_posNED;
    rngPred = deltaPosNED.length();

    // calculate measurement innovation
    innovRngBcn = rngPred - rngBcnDataDelayed.rng;

and line 438

// calculate range measurement innovation
        Vector3F deltaPosNED = receiverPos - rngBcnDataDelayed.beacon_posNED;
        deltaPosNED.z -= bcnPosOffsetNED.z;
        innovRngBcn = deltaPosNED.length() - rngBcnDataDelayed.rng;


receiverPos.x = rngBcnPosSum.x * tempVar;
receiverPos.y = rngBcnPosSum.y * tempVar;

I would say the problem could be rngBcnSum. But i am not sure. Meanwhile, i wrote also on discord, maybe someone can help me understanding the code and fix it.

This is my situation to be more clear. The H point is where the first anchor(0,0) is placed and indicated with BCN_LAT and BCN_LNG on MP. The drone moves toward the center (but as i said it doesn’t respect the anchors distances) and then comes back on the trajectory.

Please look just at the purple line on the right, is the one i am referring to.
Is this the same error of yours?
(this is the precise configuration of the anchors from google earth)

Thanks for your explanation!

Yes, error is the same, but I was thinking little different.

My hypothesis is that the drone moves from anchor position 0 (given by BCN_LAT and BCN_LNG) to the position reported by the tag. This means that if the drone is in the center of the 4 anchors, the position is moved to the center. But if the drone is in a position other than the center, for example outside the perimeter of the 4 anchors, the position will move in that direction, not towards the center. Moreover, the magnitude of this change of position depends on the difference in distance between the position reported by the tag and the initial position (position of anchor 0). If the drone is positioned below the anchor 0, the change of position is very small because the difference between the position of the tag and the position of the anchor 0 is very small. But if the drone is placed below in the opposite corner (below anchor 3) the change of position is very large since the difference in distance between these two positions is larger.

So, I do not think that the overshoot in the of position is due to a scale error, but it is the response of the EKF3 when seeing an instantaneous change of position. EKF think that the vehicle has just moved very fast creating a change of speed and even a change of attitude to later stabilize again overshooting the position given by the tag. Take this with a grain of salt, is only my hypothesis.

Attached below is my anchor setup and a screenshot of the response when starting the drone in different initial positions. In the map you can see the change in position, and in the graphs the distance to each anchor is plotted, so you can estimate the real position of the drone with respect to the 4 anchors.

Anchors positions.

Initial drone position in the center of the anchors.

Initial position under anchor 0.

Initial position under anchor 1.

Initial position under anchor 2.

Thank you, since i am not expert, i cannot confirm neither deny what have you written, but i can say that is reasonable. I’ll try the different starting positions like you did and watch the behaviour.

Moreover i have a doubt, that i haven’t solved yet, even with the help of the forum. When you connect the drone with MP, the position automatically jumps on the origin and then traslates? Since sometimes i need to manually place on the map ‘Set Home Here’ and ‘Set EKF Origin Here’. How do you start up the system? thanks for the help.

I’ll let you know any about any news on the EKF beacon problem

When I connect the drone to MP it appears on the map almost from the beginning. About 10s after startup the EK3 automatically sets the origin, and the drone appears on the map at the position indicated by the BCN parameters. Then about 40s later is when drift in position begins. And I have never had to manually place ‘Set Home Here’ and ‘Set EKF Origin Here’ on the map.

You can take the parameters from the logs I had uploaded and compare with your setup. However, we are not using exactly the same hardware, as I already commented in a previous posts I am using a Decawave system with mdek1001 tags and anchors modified with the Arduino code to behave like a Pozyx system. But the code is not identical line by line and there might be some differences.

Waiting to hear about your tests.Regards.

Hi Chris,

Have you been able to do any more tests? I’m still stuck on this point.


Sorry, this week i have been occupied with other projects. Next week i will look again at the problem. I’ll write you here if i’ll obtain some results

1 Like

Hi, it’s me.
I did more tests in these few days. I noticed the behaviour you reported: the drone moves in the correct direction with respect to the origin, but with not precise position.

Moreover, i noticed that the FuseRngBcnStatic() in the first 40 seconds is used, here the receiverPos.x and receiverPos.y are updated. However, even if the beaconPosNED is modified, nothing happens to the position on the map. Thus, i do not think the error is related to the Kalman updates.

Then, once rngBcnAlignmentCompleted is true, the IMU beacon origin is fixed (actually its values seem correct to me). At this point the drone starts moving as we noticed. In the code there are the bcnPosOffsetNED, which is updated with the values coming from the previous paragraph (receiverPos x and y), and the FuseRngBcn() function, which is very similar to the Static() one. This last similiraty makes me think that it is not this one the problem.

This leads to bcnPosOffsetNED = receiverPos - stateStruct.position analysis:
if bcnPosOffsetNED = stateStruct.position, i noticed that the drone drifts continuously in the direction with respect to the origin, and is a very rapid update (very small movements, more or less a meter or two).
if bcnPosOffsetNED = receiverPos, the drone moves to a point near the one obtained with the classic behaviour we both observed (is near since there is not the stateStruct.position factor). I think that this variable (receiverPos x and y) is not well defined, and during the Static part increases to much. Just for trial, i modified it to 0.0, but nothing changed. I am asking how can i define a float variable null if i did wrong.

This is my analysis, i saw also AP_NavEKF3_Measurements.cpp, AP_NavEKF3.cpp and AP_NavEKF3_core.cpp, but they seem correct to me. I will continue my trials on this, i do not know if you had some improvements.
@amilcarlucas do you know something about this part of the code that can help us? sorry for bothering you

Sorry, but I am not familiar with that part.