Precision landing input shaping

[Anubis (Rick)]

In Copter 4.1 and before, Precland would directly override the position controller’s position and velocity targets. Now, it uses much smoother trajectories calculated by PosControl.

In my case at least, this has greatly reduced the success rate of precision landing because the copter accelerates very slowly towards the target, so if it loses the target, it might not reacquire it before the 2 second timeout.

I want to increase the aggressiveness of the tracking trajectories, but none of the various acceleration and jerk parameters that I thought would help don’t seem to have any affect on precland. I combed through the code and I’m not totally sure why; is it using the default value for acceleration of 100 cm/s/s because AC_PosControl::set_max_speed_accel_xy is never called by mode_loiter? But mode_land does override that value and setting the relevant parameter doesn’t seem to change much, so I don’t think that makes sense.

Anyway, does anyone have insight on how I can change the aggressiveness (acceleration/jerk) of the precision landing tracking? And to be clear, this isn’t a position control tuning issue.

[rishabsingh3003 (Rishabh Singh)]

Playing around with PSC_JERK_XY would probably help you.

Try using, “raw estimator” instead of the kalman filter for prec landing estimation because that does not have the initilization bit involved, so you won’t face the 2 second time out issue. I think a new bit added to PLND_OPTIONS to remove the initilization might make sense.

[Anubis (Rick)]

@rishabsingh3003

Playing around with PSC_JERK_XY would probably help you.

That was the first one I tried. It didn’t seem to make any difference at all. I tried all LOIT_, PSC_, and WPNAV_ accel and jerk parameters.

@rishabsingh3003

Try using, “raw estimator” instead of the kalman filter

Oh, I didn’t mean to EKF warmup time, I meant the 2 second “lost the beacon” timeout. In any case, I actually set the EKF warmup time to 0 (custom firmware, not a parameter) for other reasons, so that’s not a factor here.

[rishabsingh3003 (Rishabh Singh)]

Can you upload a log? Especially with different PSC_JERK_XY?

[Anubis (Rick)]

Here are the flight logs. copter4.3.7-pl-jerk-accel-test.zip - Google Drive
Test was performed by moving the drone a few meters away from the beacon and then switching into Loiter or Land mode from Alt Hold.

Params for the first flight:

WPNAV_ACCEL      200
LOIT_BRK_JERK    2000.000000
PSC_JERK_XY      2.000000
WPNAV_JERK       2.000000

Params for the second flight:

WPNAV_ACCEL      800
LOIT_BRK_JERK    8000.000000
PSC_JERK_XY      8.000000
WPNAV_JERK       8.000000

[Anubis (Rick)]

Here’s slightly higher quality logs. Flew at higher altitude and with a greater offset from the target, so there’s a bit more data. I was hoping that fast attitude logging would increase the log rate of PSC, but oh well.

pl-accel-jerk-test-3-4.zip

For these 2 logs, I used the following values (a factor of 10 difference!)

WPNAV_ACCEL      100
LOIT_BRK_JERK    1000.000000
PSC_JERK_XY      1.000000
WPNAV_JERK       1.000000

WPNAV_ACCEL      1000
LOIT_BRK_JERK    10000.000000
PSC_JERK_XY      10.000000
WPNAV_JERK       10.000000

[Anubis (Rick)]

After some more flights and from the above logs these things are clear:

• The accel params definitely do not apply in Loiter mode, but they do in Land mode.
  • I believe this is because Loiter is not initialized with AC_PosControl::set_max_speed_accel_xy, as I mentioned before. This may be intended, but it does mean that there is a functional difference between precision loiter and precision land. According to the code, Land mode uses the WPNAV parameters.
• Even in Land mode with absurdly high param values (jerk = 50, accel = 500), the drone would always approach the target slowly. It seems that this might just be because trajectory plan is asymmetrical: it plans for high acceleration towards the target, but low acceleration when stopping.

So maybe the issue is that asymmetry. Is there a way change target acceleration response from the current slow approach (orange) into a more aggressive response with faster deceleration (blue)?

[Anubis (Rick)]

Any insights on this?
For my copters, I’ve back-ported the old 4.1 method of doing PL (using set_pos_target_xy_cm instead of the new input_pos_vel_accel_xy) because it is much more reliable. This isn’t ideal though, because the copter does a crazy maximum-effort maneuver when it first sees the beacon (it seems worse in 4.3 than it was in 4.1 for some reason).

So I’m taking another look at this to see if there’s some change I can do to make PosControl create more aggressive trajectories. Any ideas?

[radiant_bee]

I am facing the same issue with 4.4.1 firmware of sluggish behaviour, so there haven’t been any updates on that side.
For me, the major concern is the lag created between the values of commanded position and velocity by the precision landing module and the position and velocity target calculated by the position controller for the drone to achieve. Further, I do believe there is some compensatory behaviour inside position control.

I have modified the 4.4.1 firmware to log values commanded to the drone.
The above picture plots three signals:

1. the target position commanded to the drone by precision landing(PL.tPx) using AC_PrecLand::get_target_position_cm(Vector2f& ret) method
2. the PSCN.TPN and PSCN.PN represents the target calculated inside the position controller for the drone pose and the pose achieved, respectively.

Similar is the case for velocity.
I observe this laggy behaviour with both the raw sensor and Kalman filter estimator type with the moving target option enabled.
@Anubis , @rishabsingh3003, if you know about the same, could you please provide me with insights about the laggy behaviour? Do you think this is because of input shaping in some way?

Finally,
@Anubis , I was able to catch the aggressive acceleration behaviour that you mentioned earlier, which occurs as soon as the drone catches the target. It reacts aggressively to it when WPNAV_ACCEL isn’t small. I do think that it might be the initial jerk, and after the jerk, the pos control input shaping takes care of generating kinematically feasible jerk-limited inputs for the drone, due to which the drone doesn’t react aggressively afterwards.

above is the target position, target velocity and target acceleration logs for the same flight.
Please let me know what you think of the same.

[Anubis (Rick)]

@radiant_bee

Do you think this is because of input shaping in some way?

I am certain it is; the sluggishness disappears if I replace the call to input_pos_vel_accel_xy with the old set_pos_target_xy_cm from 4.1.

@radiant_bee

I was able to catch the aggressive acceleration behaviour that you mentioned earlier, which occurs as soon as the drone catches the target.

In my case, I was talking about when I was using the backported 4.1 tracking behavior - it is always very aggressive when it first sees the target because the demanded acceleration is discontinuous instead of being jerk-limited.
However, I have noticed in the past that an initial jerk can also happen for a different reason: sometimes, the plnd EKF gets bad data during the initialization period and that causes a crazy maneuver when tracking starts. That may be what is happening in your case. I actually just solved this by setting the init time to 0. It’s always performed more reliably with 0 initialization time for me.

[radiant_bee]

Hi @Leonardthall ,
Apologies for tagging you here without notifying you. I found that you had updated the function for input_pos_vel_accel_xy . We really need your guidance here.
Could you please provide insights into how input_pos_vel_accel_xy’s sluggishness can be resolved? I tried tweaking WPNAV_ACCEL, PSC_JERK_XY, and WPNAV_SPEED, but nothing worked to reduce the time lag between the signals of PL.tPx and PSCN.TPN

[Leonardthall (Leonard Hall)]

@Anubis I had a look at your logs and I don’t understand what the problem is you are describing. You seem convinced that the solution is a more aggressive aircraft response but this doesn’t make sense. A more aggressive response will make your precision landing point more noisy and make it more likely that you loose it.

Looking at your log the position controller is getting the aircraft where it is being told to be. I am seeing your range finder isn’t always doing a great job and your position estimation is pretty noisy, especially at higher altitudes.

You seem to have answered most of your own questions about the Jerk, Acceleration and Velocity limits and how they are different between Loiter and Land.

I would also suggest using PLND_EST_TYPE 0 for your testing to start with. The EKF is estimating that the landing location is moving and this could be causing problems.

@radiant_bee It looks like your aircraft is also achieving the target location but as you don’t include the log I can’t see why the target location is moving so much.

[radiant_bee]

Hi @Leonardthall ,
thank you for getting back to us.
my eventual aim is to make precision landing work with moving targets. For this, when I enable the usage of the Kalman filter with PLND_OPTIONS as 1, the targets of position and velocity controller make the drone go very aggressive, which results in the loss of target when the target is always stationary.
but if I start with a very gradual increase in the velocity of the target, the drone is able to track it, but if I suddenly stop the target from, let’s say, 2m/s, the drone is not able to respond fast enough to this change and results in losing of the target.

Further, I also tested out using the Kalman filter but set PLND_OPTIONS to 0, which translates to giving only position targets to the position controller and zero targets for both velocity and acceleration, I came across a really weird observation.
I see that the input_pos_vel_accel_xy() commands WP_NAV acceleration, which might be because the desired target that we send is really not optimized or smooth, even after processing it through the Kalman filter. Somehow I see changing WPNAV_ACC has a very much effect on the magnitude of acceleration being commanded to achieve a target position.
I made this observation for 2 cases:

1. WP_NAV_ACC low(20cm/s2) [ good performance]
2. WP_NAV_ACC normal(100cm/s2)[very bad performance]

I observed the following things by comparing the two cases with logs:

a. tracking by PID rate controller is good for the commands given to it for each case.
b. the heartbeat kind of behaviour(high-frequency inputs) to the rate controller is present in bad cases.
c. The good case had a very smooth rate target, while the other cases show very aggressive rate inputs given to the drone,
I think the aggressive inputs given in the case of normal accelerations had an effect on the system, and subsequently, the filtered position targets from the Kalman filter itself contained some oscillations, which led worsening of the overall behaviour of the system.
PL.tPx represents the original target given to position controller by Precision Landing without applying any input shaping stuff to it…
its the argument passed in the input_pos_vel_accel_xy function

Plots for WPNAV_ACC low

Plot for WPNAV_ACC normal

furthermore, the WP_NAV_ACC is only used with input shaping.
but I found that the acceleration target for the normal case is much greater than the acceleration target for the low acc case.
WP_NAV_ACC low

WPNAV_ACC normal

the higher target acceleration might be the reason behind the greater target angular rates, which only comes from increasing WPNAV_ACC.

Further, remember that all the above observations were made for static targets.
Lastly, we cant have WPNAV_ACC as low as 20cm/s2, as that is too low. The drone won’t ever be able to catch up with moving targets if the maximum acceleration is this low.

Further, there was also this weird observation:
though TPN(target position generated by position controller for itself) is very smooth, TVN(target velocity generated by position controller for itself) is not.

the good case(WPNAV_ACC = 20cm/s2):

the bad case(WPNAV_ACC = 100cm/s2):

also, @Leonardthall could you please please help us understand what input shaping is doing?
for both (WPNAV ACC cases of 20cm/s2 and 100cm/s2) there was no change wrt to the initial distance between the drone and target in horizontal plane. Why did the controller generated TPA so high to cover such small distance?

[radiant_bee]

@Leonardthall , should I share the log files with you?
I had modified the log function for precision landing in order to monitor the original signal, kalman filter filtered signal, and signal which are sent to position controller

[radiant_bee]

Also, I am using firmware version 4.4.1

[Leonardthall (Leonard Hall)]

@radiant_bee you have got a bit of a mess here. First code is more than capable of tracking a fast reference position and dealing with strong breaking.

Callisto 50 with FSO "CHAPPiE" Beacon - Test Flight, Trim 3 (May 22)

However this is reliant of having a solid and accurate position to track (the aircraft above is also tuned very well). I don’t have your log to look at but one of the key problems with higher accelerations is higher lean angle changes. This can cause noise in the estimation of the landing position. It looks like this is playing a part in your case.

You are right to start with stationary changes but your first steps is to make sure the aircraft has very good position control. Then you need to make sure the precision landing is generating a stationary target.

I personally would not use the EKF as it is unnecessarily complex.

[radiant_bee]

Hello again @Leonardthall ,
Yeah, higher lean angles with high acceleration do indeed cause a problem because I have used vision to estimate the pose of the target(I am landing on aruco markers)
The logs that I have posted above belong to the simulation.
With logs, I see that ardupilot controllers aren’t doing bad, especially the rate controller.
Further, I give manual commands through mavproxy; the drone is able to follow my commands, so I believe the position controller is working okay. Is there something I am missing here?

Using EKF helps because

1. it sort of acts as low-pass filter
2. it takes care when I do have some really abrupt changes in the actual signal due to the orientation change of the drone

Please let me know if I am missing something here so as to get the system working.

Further, I am trying to land on a vehicle from an altitude of 35m from the target.
I agree that vision introduces a lot of other uncertain factors in the system

[radiant_bee]

@Leonardthall ,
please find my logs here:
https://drive.google.com/drive/folders/1NJenL-G25j_WMVv2_mMQ0xN8XutyT1Cr
The PL log structure has the following definition:
uint64_t time_us;
float relative_pos_x;
float relative_pos_y;
float relative_vel_x;
float relative_vel_y;
float original_measurment_x;
float original_measurement_y;
float target_pos_x; // send to position controller as target
float target_pos_y; // send to position controller as target
float target_vel_x;
float target_vel_y;
float xypos_var;
float kalman_estimated_rel_pos_x;
float kalman_estimated_rel_pos_y;
float kalman_estimated_rel_vel_x;
float kalman_estimated_rel_vel_y;

[Leonardthall (Leonard Hall)]

If your precision landing position was correct then it would be moving by the same amount as the aircraft position. You should be able to offset the aircraft position by the relative position and it should be stationary. This is clearly not the case.

@radiant_bee

Using EKF helps because

1. it sort of acts as low-pass filter
2. it takes care when I do have some really abrupt changes in the actual signal due to the orientation change of the drone

If you are doing all this in simulation you should be able to ensure almost perfect data from the landing point. Make sure that all works without the EKF then you can add noise ect.

[radiant_bee]

Hi @Leonardthall ,
As you suggested, I tried to make the inputs to the precision landing as perfect as possible with the help of ground truth data I got from the gazebo.
Based on the same, there are no oscillations in the system, but the controller, I think, is still very sluggish. It takes nearly 8 seconds to achieve that position, which is a lot of time to attain a given input especially when I start testing this with moving targets.

I have attached the log below:

00000049.BIN

WPNAV parameters:
WPNAV_ACC=250cm/s2
WPNAV_SPEED=1000cm/s
WPNAV_JERK=1.5m/s2
PSC_JERK_XY=1.5m/s2