New follow-me flight task #18026

potaito · 2021-08-05T08:35:55Z

This replaces PR #17029

Describe problem solved by this pull request

This PR re-introduces the follow-target flight task which was originally located inside navigator. The goal of this new implementation is to create a more cinematic experience, assuming that the follow-target mode is used mostly for filming a moving subject.

Reasons for this PR:

Original follow-me seems not actively maintained
Since original follow-me was written in navigator, the possibilities for smooth tracking were very limited
We are generally trying to reduce the complexity of navigator and move things to standalone flight tasks.

Specific changes:

Moved follow-target logic outside of navigator into flight task
More follow perspective settings than before
New alpha-beta-gamma estimator for the target position / velocity, similar to the land_target filter for precision landing
Focus on smooth setpoint generation when target starts / stops moving, changes its
direction or a follow-target parameter in PX4 is changed.
Velocity-feedforward is ramped up when target is moving, and ramped down to zero when target
is stationary. This reduces visible twitching of the drone as well as audible changes in the motor RPM.
Consistently named the task follow-target. Before follow-me and follow-target were used as synonyms.
The existing parameters from the old follow-me implementation still work in the same way as before
Introduces a new parameter NAV_FT_ALT_M for specifying whether the drone should track in 2D (constant altitude) or 3D. The default setting is 2D tracking, since 3D tracking can be dangerous if unexpected.

Additional changes added from #19260:

Angle based follow position control, instead of using the offset vector filtering, to give a more natural feeling drone motion around the target
Second Order Filter for generating kinematically feasible / smooth trajectory, which allows smoother target tracking
Jerk limited Orbit angle trajectory control, to keep the angle control smooth 🧈
RC Adjustments that allows user to modify follow distance, perspective (angle) and height, which gets reset when the parameters for this is changed

Describe your solution

The follow-target message is now consumed by a simple estimator, that produces an estimate of the target's pose at 100Hz. A new follow-target flight task consumes this message and tracks it as smoothly as possible. The algorithm for the estimator is a simple 3-state alpha-beta-gamma filter. The notation follows the one from the book Fundamentals of Kalman Filtering (Progress in Astronautics and Aeronautics), chapter 13 - Fading-Memory Filter, p 521-550.

I can't post the source, but I suppose a screenshot of just the equations is totally acceptable:

Test data / coverage

Real Flight Logs

https://logs.px4.io/plot_app?log=c3f62bbe-2321-4bff-b4b6-0ae99a7659ed
- This is a test where I stood in one spot & moved around slightly to trigger tracked target orientation change
https://logs.px4.io/plot_app?log=6c62aad3-b777-4890-ae11-d7d8fdc627f5
- This is a test where I rode a bicycle and moved around quite a bit

SITL tests

Tests in simulation using python-mavsdk to publish the follow-target message to the simulated drone.

Target moving on a straight line by accelerating for a few seconds, and then stopping again.
The follow-distance is set to 8m, which explains the offset between drone position setpoint and the target's estimated position
The follow-perspective is set to behind

Responsiveness parameter 0.5 (default):

source: https://logs.px4.io/plot_app?log=db6f9b8f-1b75-401b-b361-fa58340e496a

Responsiveness parameter 0.9 (very lazy tracking):

source: https://logs.px4.io/plot_app?log=ae171c42-0267-4abe-aa5e-61d038807a2e

Responsiveness parameter 0.1 (very tight tracking):

Here's another SITL test where the target moves on a circle. The parameters are at their default values (responsiveness 0.5):

https://logs.px4.io/plot_app?log=c17cf75c-fb13-4c3a-ba98-15677ca30fb4

Additional context

TODO

Test on chasing Cars
Test on chasing Boats
Test on chasing Cats 🐈

src/modules/flight_mode_manager/tasks/AutoFollowTarget/FlightTaskAutoFollowTarget.cpp

src/modules/flight_mode_manager/tasks/AutoFollowTarget/follow_target_params.c

src/modules/follow_target_estimator/TargetEstimator.hpp

dagar · 2021-08-09T02:08:32Z

I've only started digging into this, but I'm wondering about the standalone follow_target_estimator.

Can we get away with it residing entirely within the FlightTask so that you only pay for it when actually in follow mode? Is there a scenario where it needs to run (or even just initialize) prior to the mode activating?

potaito · 2021-08-09T08:34:27Z

@dagar thanks for taking a look!

I've only started digging into this, but I'm wondering about the standalone follow_target_estimator.

Can we get away with it residing entirely within the FlightTask so that you only pay for it when actually in follow mode? Is there a scenario where it needs to run (or even just initialize) prior to the mode activating?

Yes, I initially had the estimator inside the flight task, but as you said there were scenarios for which I moved it to a standalone module. The reason is only the target's heading actually. With the current implementation I'm deriving the target heading from the target's velocity vector. But if the target is standing still when the flight task is activated and the estimator started, the heading will remain unknown / wrong / random until the target starts moving. The easy fix was to have the estimator running at all times so that the heading is already available when switching into the flight task. Same is true for when temporarily switching out of the follow-me flight task and back in.

The FOLLOW_TARGET message unfortunately holds no field for "heading".

Another thing I mentioned above is that I'd consider merging the estimator with the precision-landing-estimator, since they are essentially fulfilling the same task.

dagar · 2021-08-09T14:02:12Z

Yes, I initially had the estimator inside the flight task, but as you said there were scenarios for which I moved it to a standalone module. The reason is only the target's heading actually. With the current implementation I'm deriving the target heading from the target's velocity vector. But if the target is standing still when the flight task is activated and the estimator started, the heading will remain unknown / wrong / random until the target starts moving. The easy fix was to have the estimator running at all times so that the heading is already available when switching into the flight task. Same is true for when temporarily switching out of the follow-me flight task and back in.

That makes sense, we'll just need to add something so that it doesn't run by default on the older boards that are tight on memory.

The FOLLOW_TARGET message unfortunately holds no field for "heading".

Extend mavlink?

Another thing I mentioned above is that I'd consider merging the estimator with the precision-landing-estimator, since they are essentially fulfilling the same task.

If anything one of the issues there is lack of ongoing upstream testing for precision landing. It's probably worth consolidating at some point to get it locked down.

src/modules/follow_target_estimator/TargetEstimator.cpp

dagar · 2021-08-09T14:04:31Z

src/modules/follow_target_estimator/TargetEstimator.cpp

+	_last_filter_reset_timestamp = hrt_absolute_time();  // debug only
+	_last_position_fusion_timestamp = _last_velocity_fusion_timestamp = 0;
+	_last_follow_target_timestamp = 0;
+	_filter_states.x_ned_est = {NAN, NAN, NAN};


Suggested change

_filter_states.x_ned_est = {NAN, NAN, NAN};

_filter_states.x_ned_est.setAll(NAN);

Nice! Speaking of setAll... do you also know of an easier way to check if a Vector is finite in all fields? The PX4_ISFINITE(...) && PX4_ISFINITE(...) && PX4_ISFINITE(...) seem rather tedious.

bkueng · 2021-08-11T07:40:01Z

src/modules/follow_target_estimator/follow_target_estimator_main.cpp

+	TargetEstimator estimator;
+
+	while (!thread_should_exit) {
+		estimator.update();


Can you run this on a work queue?

dagar · 2021-08-24T14:42:44Z

I brought in the position_setpoint.msg cleanup from #17029 and resolved the formatting and clang-tidy failures (squashed into your initial commit), then rebased on master.

dagar · 2021-11-16T19:08:09Z

Rebased on master and a few more clang-tidy complains fixed along the way.

junwoo091400 · 2022-06-01T08:43:03Z

It's not random sweat_smile

I thought that the CI build failures may be random since on one of my PRs, the SITL test failed although my changes shouldn't have affected the SITL behavior. But now that I checked it again, I realized that it was another issue (not able to find .ulg file) 😮‍💨. I was under the wrong impression then!!

I went over the default.px4board files of the targets that are overflowing, and found the following:

Diatone target don't exist in boards/ folder anymore, but the build is somehow working: https://github.com/PX4/PX4-Autopilot/runs/6671545515?check_suite_focus=true
I find that default.px4 files differ a lot from board to board, and for example "CONFIG_MODULES_EKF2=y" is missing in kakutef7 target, but is this expected? Don't all PX4 targets require EKF2 to run the state estimation?
Also "CONFIG_DRIVERS_RC_INPUT=y" is missing from fmu-v2 and crazyflie21. Isn't this also necessary for all the targets?

Therefore I am not sure which CONFIG_* can be removed from each default.px4board 🤔 Any input on this?

junwoo091400 · 2022-06-09T13:31:13Z

The default.px4board files have been edited to remove systemcmds like tune_control and also some airframes (HIL ones, which probably won't be used) from certain boards. I tested locally that all targets build, now waiting for the CI!

junwoo091400 · 2022-06-14T13:07:54Z

The flash build size difference has been reported in the issue: #19803

But hopefully removing .hil airframes from the overflowing boards would help pass the CI build 🤷

MaEtUgR · 2022-06-15T08:35:25Z

...dules/flight_mode_manager/tasks/AutoFollowTarget/follow_target_estimator/TargetEstimator.cpp

+	// The "G" gain is equivalent to "(1-responsiveness)", but beta is required for H and K gains
+	// From alpha-beta-gamma filter equations: G = 1-beta^3
+	// Therefore: beta = (1-Gp)^(1/3) = (1-(1-responsiveness))^(1/3) = (r)^(1/3)
+	const float beta_p = std::pow((filter_gains.responsiveness), 1.0f / 3.0f);


Could this use up quite some unexpected flash? The C library power function from NuttX is presumably linked already.

MaEtUgR · 2022-06-15T08:47:10Z

msg/follow_target_status.msg

+float32 max_angular_rate_setpoint # [rad/s] Maximum angular rate setpoint calculated from remaining angle & jerk, accel settings
+float32 angular_rate_setpoint     # [rad/s] Angular rate commanded from Jerk-limited Orbit Angle trajectory for Orbit Angle
+
+float32[3] desired_position_raw   # [m] Raw 'idealistic' desired drone position if a drone could teleport from place to places


Those are 34 more fields to log which adds flash for every field name. For comparison that's roughly the same amount as the entire estimator_status message of ekf.

Right. I think a lot of those were necessary for debugging purposes, but can be removed now.

I think we can at least remove

pos_est_filtered

vel_est_filtered

raw_orbit_angle_setpoint

desired_position_raw

Maybe also the gimbal message can be removed if it's logged elsewhere already (vmount?)

@junwoo091400 what's tracked_target_course? Is that the estimated target course?

Yes it's the estimated target course!

Some of the inner fields were removed & was incorporated into the commit!

I left the desired_position_raw in there as it gives a intuitive sense on what the 'correct' follow position is, but I am also up for removing it 😉

junwoo091400 · 2022-06-16T12:52:31Z

Current build failures are arising from the fact that AutoFollowTarget class is still mentioned in the flight_mode_manager, as it is not removed by excluding the AutoFollowTarget task from being built.

Else than that, the flashes are overflowing

junwoo091400 · 2022-06-16T14:42:14Z

Removed the removing & adding back the submodule commit, as it makes git blame harder to use. Force pushed.

junwoo091400 · 2022-06-16T14:49:35Z

Squashed changes from 3 individuals into 3 different commits to clean up the history 😉

junwoo091400 · 2022-06-16T14:58:20Z

Removed some unnecessary fields of the uORB message and squashed into my commit in the latest force push!

dagar · 2022-06-16T18:50:02Z

Almost there, only one flash overflow on px4_fmu-v5_uavcanv0periph remaining.

rename follow_me_status to follow_target_status enable follow_target_estimator on skynode implement the responsiveness parameter: The responsiveness parameter should behave similarly to the previous follow-me implementation in navigator. The difference here is that there are now two separate gains for position and velocity fusion. The previous implemenation in navigator had no velocity fusion. Allow follow-me to be flown without RC SITL tests for follow-me flight task This includes: - Testing the setting for the follow-me angle - Testing that streaming position only or position and velocity measurements both work - Testing that RC override works Most of these tests are done with a simulated model of a point object that moves on a straight line. So nothing too spectacular. But it makes the test checks much easier. Since the estimator for the target actually checks new measurements and compares them to old ones, I also added random gausian noise to the measurements with a fixed seed for deterministic randomness. So repeated runs produce exactly the same results over and over. Half of the angles are still missing in MAVSDK. Need to create an upstream PR to add center left/right and rear left/right options. These and the corresponding SITL tests need to be implemented later. sitl: Increase position tolerance during follow-me Astro seems to barely exceed the current tolerance (4.3 !< 4.0) causing CI to fail. The point of the CI test is not to check the accuracy of the flight behaviour, but only the fact that the drone is doing the expected thing. So the exact value of this tolerance is not really important. follow-me: gimbal control in follow-me follow-me: create sub-routines in flight task class follow-me: use ground-dist for emergency ascent dist_bottom is only defined when a ground facing distance sensor exist. It's therefore better to use dist_ground instead, which has the distance to the home altitude if no distance sensor is available. As a consequence it will only be possible to use follow-me in a valley when the drone has a distance sensor. follow-me: point gimbal to the ground in 2D mode follow-me: another fuzzy msg handling for the estimator follow-me: bugfix in acceleration saturation limit follow-me: parameter for filter delay compensation mantis: dont use flow for terrain estimation follow-me: default responsiveness 0.5 -> 0.1 0.5 is way too jerky in real and simulated tests. flight_task: clarify comments for bottom distance follow-me: minor comment improvement follow-me: [debug] log emergency_ascent follow-me: [debug] log gimbal pitch follow-me: [debug] status values for follow-me estimator follow-me: setting for gimbal tracking mode follow-me: release gimbal control at destruction mavsdk: cosmetics 💄

@potaito

…er position and velocity filter Follow me : tidied second order filter implementation Added velocity filtered info to uORB follow target status message, and rebase to potaito/new-follow-me-task FollowMe : Rebasing and missing definition fixes on target position second order filter Follow Me : Remove Alpha filter delay compensation code, since second order filter is used for pose filtering now Followme : Remove unused target pose estimation update function Follow Target : Added Target orientation estimation logic Follow Target : Replaced offset vector based setpoint to rate controlled orbital angle control Follow Target : Bug fixes and first working version of rate based control logic, still buggy Follow Target : Added target orientation, follow angle, orbit angle value into follow_target_status uORB message for debugging Follow Target : Fix orbit angle step calculation typo bug Follow Target : Few more fixes Follow Target : Few fixes and follow angle value logging bug fix Follow Target : Added lowpass alpha filter for yaw setpoint filtering Follow Target : Remove unused filter delay compensation param Follow Target : Add Yaw setpoint filter initialization logic and bufix for when unwrap had an input of NAN angle value Follow Target : Add Yaw setpoint filtering enabler parameter Follow Target : Change Target Velocity Deadzone to 1.0 m/s, to accomodate walking speed of 1.5 m/s Follow Target : Add Orbit Tangential Velocity calculation for velocity setpoint and logging uORB topics Follow target : Fix indentation in yaw setpoint filtering logic Follow Target : Fix Follow Target Estimator timeout logic bug that was making the 2nd order pose filter reset to raw value every loop Follow Target : Remove debug printf statement for target pose filter reset check Follow Target : Add pose filter natural frequency parameter for filter testing Follow Target : Make target following side param selectable and add target pose filter natural frequency param description Follow Target : Add Terrain following altitude mode and make 2D altitude mode keep altitude relative to the home position, instead of raw local position origin Follow Target : Log follow target estimator & status at full rate for filter characteristics test Follow Target : Implementing RC control user input for Follow Target control Follow Target : edit to conform to updated unwrap_pi function Follow Target : Make follow angle, distance and height RC command input configurable Follow Target : Make Follow Target not interruptable by moving joystick in Commander Follow Target : reconfigure yaw, pitch and roll for better user experience in RC adjusting configurations, and add angular rate limit taking target distance into account Follow Target : Change RC channels used for adjustments and re-order header file for clarity Follow Target : Fix Parameters custom parent macro, since using DEFINE_PARAMETERS alone misses the parameter updates of the parent class, FlightTask Follow Target : Fix Orbit Tangential speed actually being angular rate bug, which was causing a phenomenon of drnoe getting 'dragged' towards the target velocity direction Follow Target : Final tidying and refactoring for master merge - step 1 Add more comments on header file Follow Target : tidy, remove unnecessary debug uORB elements from follow_target_status message Follow Target : Turn off Yaw filtering by default Follow Target : Tidy maximum orbital velocity calculation Follow Target : add yaw setpoint filter time constant parameter for testing and fix NAV_FT_HT title Follow Target : Add RC adjustment window logic to prevent drone not catching up the change of follow target parameters Follow Target : fixes Follow Target: PR tidy few edits remove, and update comments Follow Target : apply comments and reviews Follow Target : Edit according to review comments part 2 Follow Target : Split RC adjustment code and other refactors - Splitted the RC adjustment into follow angle, height and distance - Added Parameter change detection to reset the follow properties - Added comments and removed yaw setpoint filter enabler logic Follow Target : Modify orbit angle error bufferzone bug that was causing excessive velocity setpoints when setpoint catched up with raw orbit setpoint Follow Target : Remove buffer zone velocity ramp down logic and add acceleration and rate limited Trajectory generation library for orbit angle and velocity setpoint Follow Target : Remove internally tracked data from local scope function's parameters to simplify code Follow Target : Fix to track unwrapped orbit angle, with no wrapping Follow Target : Apply user adjustment deadzone to reduce sensitivity Follow Target : Apply suggestions from PR review round 2 by @potaito Revert submodule update changes, fall back to potaito/new-followme-task Follow Target : [Debug] Expose max vel and acceleration settings as parameters, instead of using Multicopter Position Controller 's settings Follow Target : Use matrix::isEqualF() function to compare floats Follow Target : Add Acceleration feedback enabler parameter and Velocity ramp in initial commit for overshoot phenomenon improvement Follow Target : Implement Velocity feed forward limit and debug logging values Follow Target : Apply Velocity Ramp-in for Acceleration as well & Apply it to total velocity setpoint and not just orbit tangential velocity component Follow Target : Don't set Acceleration setpoint if not commanded Follow Target : Use Jerk limited orbit angle control. Add orbit angle tracking related uORB values" Follow Target : Add Orbit Angle Setpoint Rate Tracking filter, to take into consideration for calculating velocity setpoint for trajectory generator for orbit angle Revert "Follow Target : Add Orbit Angle Setpoint Rate Tracking filter, to take into consideration for calculating velocity setpoint for trajectory generator for orbit angle" This reverts commit a3f48ac. Follow Target : Take Unfiltered target velocity into acount for target course calculation to fix overshoot orbit angle 180 deg flip problem Follow Target : Remove Yaw Filter since it doesn't make a big difference in yaw jitterness Follow Target : Remove velocity ramp in control & remove debug values from follow_target_status.msg Follow Target : Tidy Follow Target Status message logging code Follow Target : Remove jerk and acceleration settings from Follow Target orbit trajectory generation Follow Target : Change PublicationMulti into Publication, since topics published are single instances Follow Target : Edit comments to reflect changes in the final revision of Follow Target Follow Target : Apply incorrectly merged conflicts during rebase & update Sticks function usage for getThrottled() Follow Target : Apply final review comments before merge into Alessandro's PR Apply further changes from the PR review, like units Use RC Sticks' Expo() function for user adjustments to decrease sensitivity around the center (0 value) Update Function styles to lowerCamelCase And make functions const & return the params, rather than modifying them internally via pointer / reference Specify kFollowPerspective enum as uint8_t, so that it can't be set to negative value when converted from the parameter 'FLW_TGT_FP' Fix bug in updateParams() to reset internally tracked params if they actually changed. Remove unnecessary comments Fix format of the Follow Target code Fix Follow Perspective Param metadata follow-me: use new trajectory_setpoint msg Convert FollowPerspective enum into a Follow Angle float value 1. Increases flexibility in user's side, to set any arbitrary follow angle [deg] 2. Removes the need to have a dedicated Enum, which can be a hassle to make it match MAVSDK's side 3. A step in the direction of adding a proper Follow Mode (Perspective) mode support, where we can support kite mode (drone behaves as if it is hovering & getting dragged by the target with a leash) or a constant orbit angle mode (Drone always on the East / North / etc. side, for cinematic shots) Continue fixing Follow Target MAVSDK code to match MAVSDK changes - Support Follow Angle configuration instead of Follow Direction - Change follow position tolerance logic to use the follow angle *Still work in progress! Update Follow Me MAVSDK Test Code to match MAVSDK v2 spec - Add RC Adjustment Test case - Change follow direction logic to follow angle based logic completely - Cleanup on variable names and comment on code follow-me: disable SITL test Need to update MAVSDK with the following PR: mavlink/MAVSDK#1770 SITL is failing now because the follow-me perspectives are no longer defined the same way in MAVSDK and in the flight task. update copyright year follow-me: mark uORB topics optional Apply review comments more copyright years follow-me sitl test: simpler "state machine" flight_mode_manager: exclude AutoFollowTarget and Orbit on flash contrained boards Remove unnecessary follow_target_status message properties - As it eats up FLASH and consumes uLog bandwidth

- px4_fmu-v5x_rtps disable common barometers to save flash - px4_fmu-v6x_default disable telemetry drivers to save flash

junwoo091400 · 2022-06-16T20:28:38Z

Thank you! This is great 🙌🙌

dk7xe · 2022-06-18T18:56:42Z

Great to see that merged!

potaito self-assigned this Aug 5, 2021

potaito mentioned this pull request Aug 5, 2021

[WIP]: move "Follow Me" out of navigator to flight_mode_manager #17029

Closed

potaito requested a review from dagar August 5, 2021 08:36

potaito marked this pull request as draft August 5, 2021 08:41