Px4-autopilot: Pos control landing

[Stifael]

The landdetection still does not recognize landing consistently.

[Stifael]

It turns out that the stick threshold is set very tight:

Blue is the stick input which should be below 0.05.
The thrust and velocity satisfy ground contact condition.

[LorenzMeier]

We should set the calibration such that the sticks reach maximum travel at about 90% physical motion

[LorenzMeier]

So that you don't have to have it completely down

[tops4u]

Pos Ctrl Landing has never worked for me. I'd rather had some scary moments with it, when it tried to compensate some GPS deviation while on the ground. I never trusted this function and I always switch to Stabilized after touch down. This is inconvenient but works.

[dagar]

@Stifael was this fixed?

[mhkabir]

@bresch I think we were running into this today, on the field.

[Stifael]

Hm, the land detector seemed to work better. But its crucial to set the minimum thrust correctly and not to put the landing speed too low. The latter ensures that the thrust drops below the min thrust more quickly.

[bresch]

Here you have a few logs about land detection failures:

http://logs.px4.io/plot_app?log=bf1fc3b0-e88d-415d-9341-7d89c3df1450
http://logs.px4.io/plot_app?log=1f0b293c-8bf6-46eb-99c8-f4831263a5fc
http://logs.px4.io/plot_app?log=230472d3-0b70-46c1-8fd0-64de2376c0a6
http://logs.px4.io/plot_app?log=ac39ed67-05b1-4c35-8ae3-f66a96c2cc25

[MaEtUgR]

I think reaching the minimal thrust is mostly the problem (did not check the logs yet). I don't understand exactly why but we had a racer that did not even reach the ground with the default landing speed because the position controller still put out too much thrust to get the drone through the ground effect...

So I know it's only a pretty bad workaround but you can try for now to set MPC_LAND_SPEED to a higher value, descent manually with common-sense slow speed to not hit the ground too hard and then push the throttle stick down to the limit to get the thrust low...

[bresch]

@MaEtUgR You are right, the minimal thrust is the problem. In the following graph, you can see that when the drone hits the ground, the thrust only decreases slowly.

@Stifael In my case, I think that I can increase the MPC_THR_MIN parameter, but I don't want to increase it too much because I have a VTOL and if I have lift due to the wind, I'll never be able to touch the ground in position control. Am I right?

I think that could be fixed with by a velocity error monitoring, like during the landing phase of a mission (https://github.com/PX4/Firmware/blob/master/src/modules/mc_pos_control/mc_pos_control_main.cpp#L1961).

[Stifael]

@bresch
see https://github.com/PX4/Firmware/pull/6735 for similar discussion

[Stifael]

@bresch:
obviously increasing the min thrust and vel_max_z is not the solution, even it might hide the bug. monitoring the velocity is one solution, but I think there is a an actual bug in the code that prevents the vehicle to go down entirely.
If I set the MPC_VEL_MAX_DN very low, I cant even fly downward. I will look into it

[Stifael]

MPC_VEL_MAX_DN is set to 0.1. orange is thrust, purple is the estimated velocity. when the vehcile hits the ground, the estimtaed velocity becomes negative instead of zero. although this would help to bring the thrust down more quickly, I dont understand why the estimation is closer to zero. also, in the plot above I the estimation is at zero.
also, when the velocity max down is set to small number (such as 0.1), I am able to get into the state where the vehicle increases in height despite having the throttle stick all the way down. This, however, is most likely due to the integrator which is not filled up at this stage (on the other hand, it takes quite some time do until the vehicle goes down again)

[Stifael]

in sitl, from this log: http://logs.px4.io/plot_app?log=d96f06d1-8e65-4643-b702-b99176f5ed93
The max downward speed is set to 0.2. however, even demanding full downard speed, the vehicle does not move at all because it already thinks that it reached the max speed. when letting go the stick, the vehicle increases in altitude slightly.
I think there are two weird things that I dont understand:
1.) why is the estimation not converging to the true value (see plot below and compare to groundtruth)
2.) when letting go the stick and we already know that estimation is off, why then does the vehicle not increase in altitude?

maybe @priseborough already has some clue?

[bresch]

2.) when letting go the stick and we already know that estimation is off, why then does the vehicle not increase in altitude?

From my understanding, when you let go the sticks, the altitude controller will be engaged and set to the current altitude and will maintain the quad to the correct altitude even if the velocity is slightly wrong.

[Stifael]

@bresch, yep thats true. that would explain the 2.). it uses the baro then

[devbharat]

@Stifael We've solved this issue (or tried to handle estimation error on LPOS_VZ when landing) by not just controlling the constant LPSP_VZ, but using a slow integral adding to LPSP_VZ based on error in altitude (LPSP_Z - LPOS_Z) where LPSP_Z is moved downwards at the desired descent speed. It needs careful implementation, but works very well. (LPOS_Z seems to not have estimation errors like LPOS_VZ).

[Stifael]

@devbharat thank you for the explanation, that could work (or already does in your case).
By the way, the estimation for LPOS_Z can be off by several meters, but since you use the error in altitude that does not matter.
I will consider your solution, but I am still curious why the estimation in vel_z is not doing better, even in sitl

[devbharat]

@Stifael We had estimation errors right after back transition as accel bias in body_x(cruise nose direction) was being learnt in Cruise and the suddenly after back transition to Hover was being used for LPOS_VZ estimation. There was also some issue with Mag alignment(hardware mag placement aligned with Pixracer) not being perfect, and velocity innovations were quite sensitive to the magnetometer's alignment offset. @priseborough had a long look at this for us, maybe he has something to say here.

Ideally you could expect upto 0.3 m/s offset in LPOS_VZ, but in worst cases we had upto 0.6 m/s that lasted for over 10 seconds(sometimes even more).

LPOS_Z can be off by several meters, but since you use the error in altitude that does not matter

yes, but it's the change/growth in the error between the descending altitude setpoint and descending altitude that'll add to the integral. An offset in LPOS_Z would not make any difference.

[bresch]

@Stifael Do you only have the velocity estimation issue on sitl or also on the real hardware? Because I checked my logs and my velocity converges correctly to the gps velocity.
It looks like a tuning issue and I'm not sure if it's a good idea to implement a "hack" if it could work the right way with better tuning.

[priseborough]

@bresch A recurring problem with SITL has been that the vehicle simulation and FMU simulation are not synchronous which causes problems with IMU integration errors if the simulation is not able to keep up. This is particularly an issue when running the gazebo sim on slower platforms.

@Stifael Adding an integrator as @devbharat suggests is a short term fix, but I think that longer term we should be moving to a reference model approach, where both the velocity and position states of a reference model set-point are tracked and we do not have these transitions between velocity and position control. That would also enable 'stick feel' to be adjusted by changing velocity, accel and jerk limits on the reference model without changing any of the error gains. Such an approach does need logic to ensure that the reference model adjusts the trajectory if the vehicle control saturates.

[Stifael]

@bresch: im not planning on implementing anything yet, thats why I want to know why the estimation was off. I saw that you dont have the issue, but fact is that we flew other vehicles where we were not able to land unless we increases the landing speed, similar to sitl. I probably shoud not have put it here, but when I was trying to replicate the problem I found the oddness. I actually first also assumed that it ii is a tuning problem, but im not entirely sure anymore.
@priseborough this issue should not be affected by just using velocity control. When landing you never enter position control and if estimation and control work correctly then it should work. The problem with the velocity estimation is a problem that occurs in reality as well and should be independent of the controller. @devbharat fix is a fix/hack for the estimation and not for the controller, which should not be done in the controller side of course. Reference model is a different topic, but depending on how you track the reference/trajectory you might end up with same problem because you dont want to do carrot style tracking along the trajectory, but rather velocity tracking

[devbharat]

Let me summarize this from my side and exit:

1. The issue we faced (and apparently many of you are facing) is that while landing, velocity tracking is working perfectly fine, and still the aircraft does not land. This is because the velocity estimate has an offset.
2. If you look at the corresponding position estimate, you would see that it is not decreasing. ie. the estimator knows that the aircraft is not descending - which is correct.
3. There are two ways to fix this problem. First would be looking into the estimator, as to why there is an offset in the velocity estimate. We did that, it's not perfect still but significantly better. [@priseborough would know whether the changes we made were hardware specific parameter tunings in the estimator or if there were functional changes to EKF2 and if they are in upstream code already].
4. Regardless of estimator fixes I mentioned above, the problem can and should also be addressed to in the controller scheme. When landing, you _want_ to control the position, in that you want the position to descend (at a certain rate) and if your estimator tells you your position is not decreasing, and you do nothing about it, it is rather silly. Not using your position estimate for landing control makes your aircraft more susceptible to errors in velocity estimate, which is what you see. This is in contrast with @Stifael 's statement that _this issue should not be affected by just using velocity control_ , and makes modifying the control loop to use position while landing more a fundamental improvement than a hack fix.

[priseborough]

@Stifael My understanding is that using 'just velocity control' is a problem if the demanded velocity is less than the velocity error. The end result is that the vehicle does not descend.

As for estimation 'working correctly', of course we will take care with the estimator to reduce the effect of sensor errors, but if the vehicle cannot land due to a vertical velocity error of 0.3 m/s (remembering that GPS can be biased by that much), then we also need to improve robustness of the control loops.

[Stifael]

@devbharat

in that you want the position to descend (at a certain rate)

that would be the velocity.

you are completely correct that your solution is a complete valid solution. but why should the mc_pos_control be involved in that? I prefer to keep the mc_pos_control simple (its already doing way too many things), which means that it should just receive setpoints and current estimates.

@priseborough : even if the velocity error is small, the integrator should close it at some point.
by the way, the velocity estimation error might not be the root cause for having failed landings when close to ground (I did not see the log). I just realized that in sitl there is this oddness, but since it is simulation I would not make any farther claims.

@devbharat I will keep your solution in mind. but the problem that @bresch and myself encountered is still not solved.

[devbharat]

@Stifael when position estimate and velocity estimate from the estimator do not agree(like in this case when the velocity estimate has an offset), then the rate of change of position estimate is different from velocity estimate.

For example, in the flight plot below, look at LPOS_Z and LPOS_VZ. LPOS_VZ is consistently >0 and yet the LPOS_Z estimate stays at the altitude. Look also at the range sensor based altitude LPOS_Dist showing the plane not descending.

The plot below shows the "disagreement" between LPOS_VZ and GPS_velD

It can also happen the other way, causing the plane to descend faster than expected

Look at Bais estimate

and Position and velocity innovations having a jump

why should the mc_pos_control be involved in that?

Because luckily position estimate seems to not be biased ever, so using it in the controller in mpc makes sense to me.

The control Fix

For testing, we tried to mess with the velocity estimate MPC is reading by adding offsets to the LPOS_VZ in the subscription. This caused the aircraft to descend faster/slower than we'd want while landing. The controller realized that the position is descending faster/slower than expected, and modified LPSP_VZ accordingly to descend at the appropriate speed. The plot below shows the slow integral making the correction to LPSP_VZ while landing(time 947s to 965s):

[Stifael]

@devbharat I got your point from the beginning.

then the rate of change of position estimate is different from velocity estimate.

I really understand what you mean, already the first time you mentioned that. I think where we dont agree on is about how to deal with it. I prefer not to take the rate of the change of position (which comes from baro only) just because the velocity estimation (mainly from accelerometer) is off. I prefer to have them in sync from the beginning on.
That said, since you tackled this problem already and probably spent some time to figure out a solution, it therefore might be the case that your solution or a similar approach is the only thing that works. Hence I dont want to keep anyone from doing a PR for it.

[priseborough]

Something I can look at on the estimator side is provide the option in the output predictor for a vertical velocity that is kinematically consistent with the height. This will increase the effect of baro noise and position errors on height keeping but would enable use of a 'velocity only' control strategy.

[MaEtUgR]

@priseborough Not sure what you mean by output predictor, will it then just be hardcoded output different from the actual estimated states?

I know there is a clear tradeoff and fusion between using accelerometer and barometer measurements. I guess barometer is used in measurement update of the altitude state and accelerometer is used in measurement update of the vertical velocity state.

But if that's the case why is the model which is certainly including ΔAltitude = vertical velocity not "trusted"/"weighted" enough such that at least the sign of the states make sense in a kinematic sense?

[priseborough]

@MaEtUgR The ecl EKF buffers observations and runs the filter on a delayed time horizon so that the effect of measurement delay can be compensated for.

The output predictor applies a strapdown INS algorithm to IMU data at the current time horizon to calculate quaternion, velocity and position output states. The output states are buffered and compared to the EKF states at the fusion time horizon and a correction is then applied to the output state buffer so that the output states track the EKF states.

This also acts to filter the observation noise which are the small steps in EKF states produced whenever a GPS, magnetometer or baro observation is fused. The curent algorithm minimises tracking error.

I have created a PR here which provides the option to output a velocity that is equivalent to the first derivative of the position: https://github.com/PX4/ecl/pull/265. This does reduce tracking accuracy relative to the EKF states but if the mode was selected when the velocity demand was being limited or set by the landing sink rate limit, it would mask the effect of EKF vertical estimation errors.

[Stifael]

https://github.com/PX4/Firmware/pull/7152

[LorenzMeier]

This is too involved to make a last-minute change before the release.

[Stifael]

an improved mc-landdetector PR is here:
https://github.com/PX4/Firmware/pull/7314.

[Stifael]

I will close it. Because I opened this issue I will close it for now, but keep tracking the land-detector progress