AP_VisualOdom: improve Intel Realsense T265 integration

[rmackay9]

Here are a few things we could do to improve integration with the T265 discovered during my testing (blog)

• [ ] align t265 position and yaw if EKF source is GPS (PR)
• [ ] fix bug with initial yaw aligned to ahrs even if t265 is source of yaw
• [ ] fixup EKF glitch protection to catch impossible speed and/or position changes like those found in the log linked in this PR: https://github.com/ArduPilot/ardupilot/pull/15491. The glitch protection should use both position and velocity (if available) and then not fuse either if a glitch is detected. We should then remove the glitch protection from Thien's script
• [ ] determine ideal position, velocity and error values to send to the EKF and update what the scripts send and also the VISO_POS/VEL/YAW_M_NSE parameter defaults
• [ ] Re-align camera to autopilot frame if EKF yaw reset happens?
• [ ] vision-position-delta message handling of orientation is probably backwards (code is here)
• [ ] vision-position-delta message handling is not incorporating EKF update rate. We should add a line like, "timeStamp_ms -= localFilterTimeStep_ms/2;" similar to how it is done in NavEKF3_core::writeExtNavData.
• [ ] add a pre-arm check that vision-position-estimate's "reset_counter" field is non-zero (after thien's scripts are modified to always send a non-zero value). This check will ensure the system is using Mavlink2 which is a requirement for position resets to work
• [ ] add a pre-arm check that T265's confidence is "High"
• [ ] "GPS Glitch" / "GPS Glitch cleared" message appearing after ekf-origin is set (at least in SITL using EKF2)
• [ ] check if APSync's real-time-clock is working (may need this hardware: https://chicagodist.com/products/rasclock-rtc-for-raspberry-pi)
• [ ] add support for multiple T265 cameras (brought up in this discussion)

Complete:

• [x] SITL should be enhanced to be able to send vision-position-delta messages (issue)
• [x] allow filtering out vision-position-estimate, vision-speed-estimate messages so they are not passed to GCS (two solutions may be required depending if telemetry system is connected to the autopilot or to the companion computer) (PR https://github.com/ArduPilot/ardupilot/pull/14742)
• [x] release new APSync image for RPI4 that includes all latest script enhancements and update librealsense version to v2.35.1 (https://github.com/ArduPilot/companion/blob/master/Common/Ubuntu/librealsense/install_librealsense.sh#L16)
• [x] add NaN checks for vision-position-estimate, vision-speed-estimate and vision-position-delta (PR)
• [x] add VISO_POS_M_NSE and VISO_YAW_M_NSE parameters to apply minimums to noise params sent to EKF
• [x] Integrate camera's reported accuracy into the EKF
• [x] allow higher rate of external nav messages (14hz is current max, raise to 40hz or 50hz)
• [x] add support for VISION_SPEED_ESTIMATE and/or ODOMETRY (PR1, PR2)
• [x] add SITL support for sending VISION_SPEED_ESTIMATE (see PR2 above)
• [x] Add glitch protection for external nav position like we have for GPSs (PR)
• [x] Add VISO_LAG parameter and use in EKF2/EKF3 (PR)
• [x] Add SIM_VICON_YAWERR for yaw glitch testing (PR)
• [x] fix EKF2 yaw initialisation (PR from chobitsfan)
• [x] Improve incorrect yaw handling (issue, PR)
• [x] Fix EKF3 pre-arm check if external nav used as yaw source (issue)
• [x] Measure lag between camera and autopilot IMU and ensure EKF is using the correct lag. Appears to be 15ms based on log analysis.
• [x] Move handling of the mavlink message to AP_VisualOdom (issue)
• [x] Calibration to rotate the camera's frame (for attitude and position) into the AHRS/EFK's frame. This includes rotating the camera frame to match the AHRS/EKF compass.
• [x] Pre-arm check to ensure the camera's reported attitude is close to the AHRS attitude
• [x] Handle camera reported resets of attitude or position. We apparently have some fields in the mavlink messages now (link). This EKF bug https://github.com/ArduPilot/ardupilot/issues/14046 prevents us from handling the camera's position resets.

---

Here is the corresponding to-do list for Thien's T265 scripts

[rmackay9]

PR https://github.com/ArduPilot/ardupilot/pull/13982 resolved 3 of the issue above

[anbello]

Regarding first point under "Complete:" "Measure lag between camera and autopilot IMU and ensure EKF is using the correct lag."

Where was this implemented? In the code? As parameter? For EKF2 we have EK2_EXTNAV_DELAY param, but I don't see the analogous for EKF3. It should be useful as parameter to adjust it for different device or different method to send pose estimate.

[rmackay9]

@anbello, Tridge has a script that can estimate the lag. I'm not sure where it is but it came up with 15ms.

Thanks for the reminder on the lag for EKF3. I've added an item into the list above.

[60ericleal60]

I am unsure if this is the right place to discuss this because it may be ground station software related but I am having issues with viso offset values with the t265 camera and the APSync image. I have an airframe where the t265 camera and imu are pretty far apart (250mm) and the imu is mounted at the mass center which I select as the vehicle origin and place all of the other sensors offsets from this origin. If the vehicle is rolled right about its mass center viso y moves left and local position ned slowly moves to that position as if the viso offsets do not exist. As an experiment I set the camera position very above and below the imu (+/- 1.5m) with no change in result. If the t265 camera is set as the vehicle origin with the imu given a position offset the position ned tracks with is viso values very quickly. After updating the firmware to today's file the viso position parameter no longer shows up on the ground station even after updating to Mission Planner Beta. Is the implementation of offset and orientation being moved?

[rmackay9]

@60ericleal60, The t265 support is our stable release is a bit weak so the best place to discuss may be on the developer gitter channel although some other developers may object: https://gitter.im/ArduPilot/VisionProjects

Are you sure that you've got Copter-4.0.4-dev on the board? I suspect you've somehow installed Copter-4.0.3.

The test you're running is interesting, maybe you could provide a log file?

[60ericleal60]

Here are some logs. 19 is an actual flight where the vehicle was erratic in position mode. I have a smaller airframe where the camera offset is small which flies very well. In 20 and 21 the propellers are off and I am rolling the vehicle about its mass center. In 20 there is a camera offset and in 21 there is not. viso_flights.zip

[60ericleal60]

When I update the firmware the viso offset parameters go away

[rmackay9]

@60ericleal60,

Ah, you just need to set the VISO_TYPE to 2 and refresh the parameters. This is a common thing we do in ArduPilot to hide parameters for users who aren't using the feature.

I've discussed with the other devs and you're more than welcome to join us on the gitter VisionProjects channel. https://gitter.im/ArduPilot/VisionProjects

[60ericleal60]

Thank you that worked and the controller now appears to be using those offsets. I then tried flying the vehicle and its behavior in hover in position hold was erratic. It looks as if the position controller isn't outputting anything but I am unsure. This is my first time flying 4.0.4. I think the first thing I will check is if it still does well in GPS aided position hold. 00000022.zip

[rmackay9]

@60ericleal60, looks like there may be issues with the RC input. Maybe you're using a joystick instead of a normal transmitter?

I also see that the EK2_ALT_SOURCE = 1 (for rangefinder). If you're really flying indoors this may be OK but otherwise I would return it to zero.

I agree it's best to go back and test with regular GPS. It's important that you've got a good solid working vehicle otherwise we spend our support time on debugging issues unrelated to the non-GPS vision feature.

[60ericleal60]

I am reading an frsky radio with a microcontroller and then augmenting 2 of the channels for obstacle avoidance. I have flown successfully with this method on px4 for a long time and had relied on the pixhawk checking the first and last bytes of that packet for loss of link and corrupted packets. For now I will bypass this.

Because the t265 outputs a z position should I remove the lidar?

[rmackay9]

@60ericleal60, I would leave the lidar on the vehicle for now. It is a good extra source of altitude information and this is useful for determining if the camera's scaling is correct (sometimes it's not and we are still investigating how to protect against this). If the vehicle is being flow indoors on a vehicle with short legs sometimes the barometer altitude also can't be trusted so having a 3rd source is valuable.

[60ericleal60]

Flight 0022 above is outdoors so there is so change in terrain. Is there an issue with the viso z position and range finder distance moving away from eachother?

Here are some flights on my smaller airframe indoors with a flat floor. It is on 4.0.3 and I suspect the sensor offset is small enough not to be catastrophic. I have set the position gains quite high here so I am not terribly concerned with the large changes in attitude.

https://www.youtube.com/watch?v=a0IZEzS0GoI&feature=youtu.be https://www.youtube.com/watch?v=zxfDPDcYQ74&feature=youtu.be

mini_quad_viso.zip

[60ericleal60]

After starting over on the vehicle setup with 4.0.4 and excluding the ground laser ranger, I am still unsure if the sensor offset values are being used. It appears like the offset compensation may occer on the raspberry pi from the mavlink message vision position delta but those values are 0 no matter what sensor offsets I put in. Do the vision sensor position offsets get calculated on the flight controller of the companion computer?

[amilcarlucas]

I think https://github.com/ArduPilot/ardupilot/pull/16627 belongs in that list.

[rmackay9]

@amilcarlucas thanks for bringing this up but I don't think it is directly related. The T265 provides a local position estimate but we put this together with the EKF origin (lat, lon, alt) to determine the vehicle's global position.

Chobitsfan's PR adds support for missions to be specified in a local frame but this is useful regardless of whether the vehicle is using GPS, T265, optical flow or whatever else.

[amilcarlucas]

My point here is that:

• T265 will be mostly used in indoor environments
• indoor environments are usually described in NED [m] coordinates and not in lat, lon, alt
• the user setpoints should therefore be given in NED
• the copter will of course internally continue to use lat,lon ,alt

But yes, this is not directly connected to T265, but it is still very relevant IMHO

[shubham-shahh]

My point here is that:

• T265 will be mostly used in indoor environments
• indoor environments are usually described in NED [m] coordinates and not in lat, lon, alt
• the user setpoints should therefore be given in NED
• the copter will of course internally continue to use lat,lon ,alt

But yes, this is not directly connected to T265, but it is still very relevant IMHO

Hi, in my opinion, the T265 implementation with ardupilot is just an example of how external SLAM algorithms can be used with ardupilot, that said SLAM algorithms are also used for outdoor navigation, so implementing it as (indoor-only) is not a good thing in long run

[rmackay9]

@shubham-shahh,

Yes, I think I agree. I think AP can use SLAM (including the T265) both indoors and outdoors (or a combination of the two in a single flight). We should still add the ability to allow users to specify mission commands using offsets from the EKF origin but that will be in addition to what we have now (lat, lon, alt).