VIO alignment in primary lane

ArduCopter Copter 4.1
9

Good morning @rmackay9,

I gave it a try with the modification I mentioned, and there are a couple things worthwhile sharing. Before sharing some discoveries, I want to point that one of my objectives was to be able to have certain logic that aligns the T265 yaw (source 1) with the GPS yaw (source 0) automatically without using a the RCx_OPTION=80 channel. These are my findings:

  1. It was not difficult to create necessary Lua binders for aligning the visual odometry since the class AP_VisualOdom is implemented as singleton; it is my understanding that the binders generator uses the same pointer that avoids the creation of more than one object of a singleton class, very clever. Thus, for having this feature available on the Lua script it was only required to add to AP_Scripting/generator/description/bindings.desc:
include AP_VisualOdom/AP_VisualOdom.h

singleton AP_VisualOdom alias visualodom
singleton AP_VisualOdom method healthy boolean
singleton AP_VisualOdom method align_sensor_to_vehicle void

May be these are binders that can be there by default. The only problem is that there is no way to know if the alignment was successful, unless I switch sources momentarily and check that the yaw has no an abrupt jump. This is because the yaw of the T265 apparently is not saved anywhere, but it is corrected using the attribute yaw_trim of the class AP_VisualOdomT265 every time a visual_position_estimation message arrives; the align_sensor_to_vehicle is just an adjustment to yaw_trim. Thus, I could not find any method to use as a binder for the Lua script and check that the alignment happened.

  1. The check done calling AP::ahrs().yaw_initialised inside AP_VisualOdom_IntelT265::align_sensor_to_vehicle is actually very important. Getting RTK fixed does not guarantee that the yaw has been aligned in the EKF3, it usually takes a few seconds. Thus, if the align_sensor_to_vehicle method is called in the Lua script, it may happen that the yaw is not properly align in the AHRS. This is not checked I remove this check from AP_VisualOdom_IntelT265::align_sensor_to_vehicle to make it possible the alignment with GPS yaw.

  2. A work around was to create a binder for the method NavEKF3:yawAlignmentComplete(). Since this class is not singleton and for keeping consistency, it required me to create a virtual method in AP_AHRS that can be override by a method in AP_AHRS_NavEKF which finally calls NavEKF3:yawAlignmentComplete(). Something similar to what is done with the method AP_AHRS::get_vel_innovations_and_variances_for_source. Now this binder allows me to check if the AHRS with the first source (GPS yaw) has been aligned and align it from the Lua script.

I still have a question, since it is not possible to know if the alignment has been performed correctly or to check that it keeps aligned during flight, how critical is this for the T265 yaw? so far we have not seen any significant yaw drifting in the logs between the GPS yaw and the yaw in VISP, but do you think is a good idea to have something that check for the alignment of the camera during flight and realign or to call the align_sensor_to_vehicle binder periodically?

2 Likes