Altitude change when braking (compensation script)

When stopping the drone after having horizontal velocity, there is a noticeable decrease in height (2-4m), which is regained a couple of seconds after stopping. The vertical speed is only ~6m/s in these flights. This indicates that the barometer is affected significantly when stopping.

According to the PX4 documentation Correction for Static Pressure Position Error this can be compensated by running a python script on a specific log file and adjusting the parameters:

  • EKF2_PCOEF_XP = [-0.5, 0.5]
  • EKF2_PCOEF_XN = [-0.5, 0.5]
  • EKF2_PCOEF_YP = [-0.5, 0.5]
  • EKF2_PCOEF_YN = [-0.5, 0.5]
  • EKF2_PCOEF_Z = [-0.5, 0.5]

However, the estimated parameters for my drone are outside the allowed range even though the estimated corrections looks OK in my eyes. The height is still wrongly estimated when demanding vertical velocities, when compared to GP,S and is not corrected by the tuning script
Log (no wind): https://review.px4.io/plot_app?log=cb1a6965-07ad-4a1f-a2a1-59195b77638a

Tuning script output

Optimization terminated successfully.
         Current function value: 1810.789198
         Iterations: 441
         Function evaluations: 701
param set EKF2_PCOEF_XN -2.735
param set EKF2_PCOEF_XP -0.361
param set EKF2_PCOEF_YN -2.943
param set EKF2_PCOEF_YP -2.420
param set EKF2_PCOEF_Z -10.185

Questions:

  • Are these limits perhaps suited for smaller drones like x500 or should my larger drone also be in the [-0.5, 0.5] range, indicating something is wrong with my setup or flight?
  • Are drag specific forces tuning required before tuning the Correction of Static Pressure Error. The documentation states For multi-rotors, fusion of Drag Specific Forces can be enabled and tuned to provide the required wind velocity state estimates. ?

My goal is to have the drone not lose 2-4m altitude when braking to a stop.

Setup:

  • PX4 1.15.4
  • Cube Orange +
  • X style
  • Height reference: Baro
  • TOW = [12-20kg]
  • GPS = UM982 (dual antenna)

If it helps I also run the wind estimator tuning script.
Output when running mc_wind_estimator_tuning.py results in parameters in allowed range except EKF2_DRAG_NOISE which is below the minimum of 0.5.

Optimization terminated successfully.
         Current function value: 250.423765
         Iterations: 79
         Function evaluations: 141
param set EKF2_BCOEF_X 47.2
param set EKF2_BCOEF_Y 51.9
param set EKF2_MCOEF 0.07
/!\EXPERIMENTAL param set EKF2_DRAG_NOISE 0.05
  • EKF2_BCOEF_X = [0, 200] kg/m^2
  • EKF2_BCOEF_Y = [0, 200] kg/m^2
  • EKF2_MCOEF = [0, 1] 1/s
  • EKF2_DRAG_NOISE = [0.5, 10] (m/s^2)^2

I added this to the dev call agenda (tomorrow 8AM PT). I haven’t used this feature since 2019, but back then it never really worked. I can’t say much more than that. Hopefully someone who knows more than me like @MaEtUgR or @bresch can comment.

1 Like

Thanks for bringing it up. Am I allowed to join in on the meeting to listen in? If not, and the subject is brought up, would you mind posting the results?

Of course you can join the call on Discord to listen and ask your question, see

On the topic:

  1. Are you sure it’s losing altitude because of barometer error? Just asking to rule out any other influence. We can double-check the log :eyes:
  2. Where’s your barometer mounted? The more bernoulli effect is generated e.g. with a high airspeed over a small hole the worse the performance and hence necessary corrections. I don’t have a feeling for the coefficient numbers but I can imagine that depending on the barometer placing and the housing of your avionics the effects can be quite strong. It’s always advisable to fix the hardware first but if the high influence remains it could be theoretically possible that a lot of correction is needed.
  3. Did you compare with another log if the results are somewhat reproducible? The pattern I usually fly is Altitude mode accelerating with full tilt in each direction and then letting it coast for as far as possible e.g. until it stops or I run out of space or visibility. Similar like for the drag, wind fusion which I usually do first.

That said I’m only user of these features and don’t know the internals so @bresch could probably correct me here.

From a quick glance at the log there seems to be a lot of barometer influence, the data looks somewhat noisy and the vehicle measures going up based on barometer with higher wind speed.

So I’d suggest:

  • Check if you can improve the hardware e.g. barometer placement.
  • Apply the high correction coefficients, especially if you verified them to be somewhat reproducible from two logs.
  • Compare to setting the GPS as primary altitude source see EKF2_HGT_REF . Note that I’d make the baro work because using GPS altitude has its own problems especially if you ever have any GPS inaccuracy because of obstruction, multi-path or any other jamming source.

You could also play with the barometer measurement noise EKF2_BARO_NOISE if it’s not as accurate.

1 Like

We can see that there is an offset of about 0.3m/s^2 (on the Right axis) between the predicted and measured accelerometer data. This would create a wind offset that can affect the baro compensation.

Do you also have a log with this tuning applied? We should verify that in calm conditions, the wind estimate should stay close to 0 regardless of the speed and direction of the drone.

One potential issue is the level horizon; if the autopilot is leveled correctly, you can set SENS_BOARD_[XYZ]_OFF back to 0

Regarding the pressure coefficients, EKF2_PCOEF_Z is probably unobservable as you can see that the fit is really bad at the end of the flight, when you only perform altitude changes. → you can thus ignore that result and leave it to 0

You can also lock down this parameter to 0 in the script to refine the other parameters. To do that, add bounds to the optimizer:

`res = optimize.minimize(J, x0, method=ā€˜nelder-mead’, bounds=[(-5,5),(-5,5),(-5,5),(-5,5),(0,0)], options={ā€˜disp’: True})`

1 Like

Nice, I’ll make sure to join the meeting then.

  1. I am certain it has to do with barometer error. I performed multiple flights today where I initially used barometer as primary height source EKF2_HGT_REF = 0 (baro) and observed the problem. I then switched to EKF2_HGT_REF = 1 (gps) and disabled baro aiding via EKF2_BARO_CTRL = 0 (disabled) , and did not experience the altitude drop to the same extent. Using baro drop is about 1-4m, using gps it’s below 0.5m, which isn’t really noticeable.
  2. Barometer is the internal Cube Orange+ which is mounted inside the airframe body (not an airtight frame but fraily water resistant (IP54). The body uses pressure/vent/moisture plugs to allow pressure changes and moisture to escape. It’s similar to this part form digikey. I am not sure how I can improve barometer placement, it’s not in the open air. Do you have any suggestions @MaEtUgR ?
  3. The phenomenon is reproducible between multiple different aircrafts. I performed some altitude flights with this in mind to test running the script with again. Here is a flight with the ā€œCoastingā€: https://review.px4.io/plot_app?log=ac9d0380-3089-4824-bab3-f9d3211a77ab

Do you know if the wind estimation using drag specific forces are required for baro compensation to work? (usingEKF2_DRAG_CTRLand associated parameters)

I adapted the tuning script with constraints on not optimizing Z and left out the height change region of the script, and got the parameters:

EKF2_PCOEF_XN -2.777
EKF2_PCOEF_XP -0.518
EKF2_PCOEF_YN -3.126
EKF2_PCOEF_YP -2.578
EKF2_PCOEF_Z 0.000

I then did two test flights:

  1. Without the compensation parameters: https://review.px4.io/plot_app?log=a98902d4-84fb-444b-b0b3-4a06cc9d7ffc
  2. With compensation parameters: https://review.px4.io/plot_app?log=3943ee98-446b-4d8a-89b6-a7daedeaa6e0

The problem was not mitigated nor reduced, only when switching to GPS is it solved (EKF2_HGT_REF = 1 (gps) and EKF2_BARO_CTRL = 0 (disabled).

Hence, I believe this drag-specific forces (wind estimation) might need to be enabled for this compensation to work, as outlined in my initial questions. I will try that compensation script and enable Multicopter Wind Estimation using Drag Specific Forces via EKF2_DRAG_CTRL and see if that is the required action.

Let me see if I understand the wind estimation correctly.

With a correct wind estimation tune using EKF2_DRAG_CTRL, EKF2_BCOEF_X, EKF2_BCOEF_Y EKF2_MCOEF and EKF2_DRAG_NOISE the estimated wind estimate should be close to 0 even if the drone is flying with some horizontal / vertical speed. The estimate is just the wind speed then, make sense.

The autopilot is mounted on vibration dampeners and was perhaps leveled on a flat, but not perfectly flat, surface. Offsets are quite small at SENS_BOARD_X_OFF = 1.08, SENS_BOARD_Y_OFF = 0.038 and SENS_BOARD_Z_OFF = 0.0

Using the ā€œcoastingā€ flight (higher velocities, no sudden stops, just coast until stop) the parameters are a lot closer to the suggested range by PX4 documentation, but the estimate looks worse in my eyes. Especially since the fitted model only estimated the error well during the dynamic part and not the coasting, see my red mark. Could there be a problem with my airframe? Perhaps my slightly ā€œsealedā€ box doesn’t change pressure fast enough and the built up pressure takes time to be released?

EKF2_PCOEF_XN -0.592
EKF2_PCOEF_XP -0.957
EKF2_PCOEF_YN -0.580
EKF2_PCOEF_YP -0.670
EKF2_PCOEF_Z 0.000

Using the same log for wind estimation seems to produce reasonable results.

EKF2_BCOEF_X 79.9
EKF2_BCOEF_Y 72.0
EKF2_MCOEF 0.08
EKF2_DRAG_NOISE 0.04

The drag noise is however, a magnitude lower than the minimum suggested in PX4 Is it still a reasonable number? @bresch EKF2_DRAG_NOISE = [0.5, 10] (2.5 default) (m/s^2)^2
I don’t really have an intuition about this parameters other than looking at the estimated model outputs.

If that is the case I will apply the following parameters and see if I observe any improvement tomorrow:

EKF2_DRAG_CTRL 1 (enabled)
EKF2_BCOEF_X 79.9
EKF2_BCOEF_Y 72.0
EKF2_MCOEF 0.08
EKF2_DRAG_NOISE 0.04 (edit: keep at default 2.5)

EKF2_PCOEF_XN -0.592
EKF2_PCOEF_XP -0.957
EKF2_PCOEF_YN -0.580
EKF2_PCOEF_YP -0.670
EKF2_PCOEF_Z 0.000

Follow up after Dev Call.

The very low estimated EKF2_DRAG_NOISE parameter should not be used since it could make the EKF trust it too much.

1 Like