Servers by jDrones

VIO alignment in primary lane

Good morning everyone,

We gave it a try yesterday to a primary lane of EKF with VIO as yaw source and GPS for position. We noticed that the alignment for the yaw of the VIO to be referenced with North direction as zero is done with respect to the yaw is currently in use. Before taking off, this would be, and correct me if I am wrong, the yaw estimation in the first lane (EKF3 core with index 0) since the AP_NavEKF3::updateFilter method forces it to be the primary if the vehicle has not been armed. Therefore, when we define the EK3_SRC1_YAW=6 (Extnav yaw) and RC9_OPTION=80 (VisoAlign) and flip the channel 9 before flying, the alignment does not happen. In fact, it makes sense since the method AP_VisualOdom_IntelT265::align_sensor_to_vehicle check first if the source of yaw is external navigation and does not perform the alignment if this is true.

The alignment works perfectly before taking off if the primary lane has another yaw source such compass as shown below:

And I dare say that we were able to have a very good flight with the yaw of the VIO as source without the alignment making sure that the vehicle was pointing north before taking off. However, I wonder if there is a work-around to this situation that does not imply us to point to north before taking off. The yaw of the camera is quite good in certain environments where the compass is not an option. I am going to try today a Lua script that will change the primary core before taking off, so the alignment can happen having in a secondary lane the compass and making it the primary just before taking off. Not sure if it is going to work since the AP_NavEKF3::updateFilter does not allow lane switching before arming.

1 Like

Hi @sbaccam,

Really happy that you’re testing this out and that it is working OK so far.

One clarification is the EKF “source set” is a separate feature from the “lane”. All lanes will use the active source set. As a side note we have discussed adding an option so that each lane uses a separate source set but that’s still a to-do.

As I think you’ve found, the position of the external nav system is aligned to the EKF’s position whenever the external nav is not the active position source. The external nav’s yaw alignment however only happens once at startup or when the pilot moves the “VisoAlign” aux switch (assuming it has been setup). We have an item on the to-do to align the yaw as well whenever it’s not the active yaw source but I just haven’t quite gotten to it yet.

There is unfortunately no way to manually set the external nav’s yaw to a particular value so the best you can do is, before takeoff, momentarily switch the source set to one that uses compass for yaw, move the VisoAlign switch and then switch the source set back to the external nav yaw.

I’m open to ideas to make this better though! We could add a lua script method to set the external nav yaw to a particular value.

1 Like

That will be awesome, I will keep reading the source code for understanding better in order to make an educate suggestion. We tested today the lane switching between camera yaw and compass in altitude hold and loiter mode. There is something that we notice that deserves some attention, but I am going to report it in a new topic for not talking about different topics in the same stream.

Good evening,

Today we also tried a lane switch between a lane with the first source for yaw as GPS (EK3_SRC1_YAW=2) and a second lane with VIO as yaw source (EK3_SRC2_YAW=6). We were unable to align the yaw of the camera using a channel of the transmitter (RCx_OPTION=80) as we have done before when the source 1 is compass. I guess the initial alignment that @rmackay9 did mention did not happen in this scenario since the yaw requires RTK fixed which can several seconds to happen depending on your location, the weather, the position of the satellites, etc. In the following plot, it is clear that the alignment does not happen, and that when we switch between source 1 and source 2, there is a jump in the yaw:

Actually, because the misalignment, we get the message PreArm: VisOdom:yaw diff 81.4 deg (>10) at about 80 seconds, just after we tried to align several times with the channel that has RCx_OPTION=80. The log of this ground test can be found in the link below:

Anyway, we aligned the drone facing north before taking off, and had a pretty clean flight with manual lane switching. This new version of Arducopter is great and this source switching is a big improvement. Thank you all that make this great software possible!

1 Like


I’ve never actually seen a log of a vehicle transitioning between GPS-for-yaw and External Nav so this is a first! Nicely done.

So it appears that the source switching is working. We see the yaw jump and the EKF_YAW_RESET events appearing properly in the logs.

… but we can also see that, as you have described, the visual odometry yaw is not being aligned to the AHRS/EKF’s yaw when visual odometry is not being used… and the issue is just that AP_AHRS::is_ext_nav_used_for_yaw() unfortunately returns “true” if GPS-for-yaw is being used.

These are the lines in AP_VisualOdom_IntelT265 that are causing the trouble:

// use sensor provided attitude to calculate rotation to align sensor with AHRS/EKF attitude
bool AP_VisualOdom_IntelT265::align_sensor_to_vehicle(const Vector3f &position, const Quaternion &attitude)
    /// do not align to ahrs if it is using us as its yaw source
    if (AP::ahrs().is_ext_nav_used_for_yaw()) {
        return false;

This issue is already on the to-do list (see top item here) and hopefully we can get it fixed in the near-ish future. If you’re comfortable compiling the code yourself you could simply remove those lines… you will want to try and avoid aligning the yaw if the ExtNav is already the source of yaw but even if you did accidentally re-align it to itself it shouldn’t cause significant problems.

1 Like

We will try this tomorrow, and we will post how the testing goes.

1 Like

Good afternoon,

It has been a while, but we discovered what may be the issue. First, we tried removing the check @rmackay9 suggested, and still we were not able to align it. At some point I thought it was that the method AP_VisualOdom_IntelT265::align_sensor_to_vehicle was not called for some reason when the channel configured with RCx_OPTION=80 was switched. However, setting a breakpoint inside the method using GDB proved us wrong. The problem was not only on that check but also in the second check of AP_VisualOdom_IntelT265::align_sensor_to_vehicle. Apparently, when using GPS yaw, the method AP::ahrs().yaw_initialised() still returns false even if we got RTK fixed. This is the result of a ground test where we created binders for AP::ahrs().is_ext_nav_used_for_yaw() and AP::ahrs().yaw_initialised. If AP::ahrs().yaw_initialised was false, the Lua script sends the message:

“GPS yaw has not been initialized and cannot be used for alignment!”

Otherwise, the script sends the message:

“GPS yaw has been initialized and can be used for alignment!”

Similarly, with the method AP::ahrs().is_ext_nav_used_for_yaw(), if it returns true the Lua script sends the message:

“External navigation is used for yaw!”


“External navigation is not used for yaw!”

Here the log, when even after getting RTK fixed the AP::ahrs().is_ext_nav_used_for_yaw() was returning false, and the method AP::ahrs().is_ext_nav_used_for_yaw() always returns true:

Running the same test but removing the two checks, we were able to align using GPS yaw:

In conclusion, the two mentioned methods are avoiding the alignment to happen. I think for now I will be just double-check with a Lua script that the yaw has a valid value before alignment and remove the two checks. I wonder if this can be problematic for any reason.

1 Like


Thanks for the detailed analysis and feedback. I think the work around you’ve put in place seems OK to me until we get a proper fix in place.

1 Like
Servers by jDrones