Wind Estimator Tuning Equation

Hello,

I just realized I posted this in the wrong place before, but I hope someone can possibly help me on here!

I’m trying to figure out where the first half of this equation for predicted acceleration came from. I understand that the second half is simply the drag force equation simplified to be in terms of the drag acceleration and the ballistic coefficient, but I’m not sure where the (-v * Mcoeff) would come from (where x[0] = Mcoeff = Momentum Drag Coefficient).

I’ve googled it, read through some seemingly related research papers, but haven’t been able to find anything relating a specific Mcoeff to acceleration by multiplying it by the body frame velocity in each direction, as seen here. Additionally, I’ve asked an aerospace engineer that I know and he can’t seem to place it either. I’m not saying that it’s wrong, as the tuning seems to work very well in a gazebo simulation, but I just want to know what it’s based on for future reference.

Thanks in advance for any pointing in the right direction.

https://github.com/PX4/PX4-Autopilot/blob/479c85047fdee9fcc44d9143dfa2bdaa96040ad0/src/modules/ekf2/EKF/python/tuning_tools/mc_wind_estimator/mc_wind_estimator_tuning.py#L135-L139

Hi @Nick_Kakavitsas ,

You can find a description of the momentum drag in this publication for example: https://www.researchgate.net/publication/223820505_Modeling_and_attitude_control_analysis_of_a_ducted-fan_micro_aerial_vehicle
This unusual type of drag is basically due to the change in direction of the lateral airflow going through the propeller; It is a dominant drag force at low speed on hovering vehicles with ducted propeller but is also observable when there is no duct. After writing this tuning script and testing it on several vehicles, we even found out that the body drag was almost always negligible compared to the momentum drag below 15m/s of relative airspeed.

I hope this helped

1 Like

Hi @bresch

That helps a lot! It’s not something I’ve thought of, but from a cursory look it seems to make sense. I’ll continue reading about momentum drag, and will try to implement it into my current drag model to try and improve my predictions.

Thank you for the help!

Best,
Nick

Hi @bresch,

I have a new question concerning the implementation of the mc_wind_estimator.py tuning script. I believe that it works very well, but to make sure I’m tuning consistently and correctly, I want to make sure I understand what’s going on behind the scenes.

With regards to what’s going on, from my understanding after reviewing the tuning file, it seems like you used a cost function which seems to find the magnitude of the differences between the measured vs predicted acceleration in X and Y. The function is then minimized with Nelder-Mead, the results of which are then converted to the Bcoeff parameters used in the estimator.

Is this essentially a way that you’re trying to match the estimated acceleration to the measured acceleration by minimizing the error between the two via varying x[0] (Mcoeff), x[1] (Bcoeff_X) and x[2] (Bcoeff_Y)? Please find the code in question in the screenshot and the link below:

Thank you for the help and guidance in advance!
Nick

Yes, basically, the drag model gives you the acceleration you should measure given the current velocity and the coefficients.
The optimization then tries to find the best coefficients to minimize the error (squared) between the acceleration predicted by the drag model and the measured acceleration.

Did you already test the script on real data? Does it give you good results?

Thank you, that makes sense.

I’ve tried it several times with the iris gazebo simulation frame, and have tried to run the calibration process in a drone net. The gazebo simulation calibrations ended up giving very good results, using an ideal calibration trajectory, but the results using the real drone in the net were relatively inconclusive. In the simulation, it was easy to do a relatively perfect calibration trajectory, but it’s quite difficult in a long thin net.

With the gazebo iris calibrations, using coefficients obtained from the tuning script gave relatively good results with the given wind field in the associated world file. The results from the ekf wind estimator with those coefficients gave much better values than I could otherwise obtain so far with my attempted direct estimation methods.

I’ve recently tried to run the gazebo calibration with the same space constraints as the net with the real drone. Running these limited space trajectories through the calibration script gave different values from the coefficients obtained by a more ideal full forward + lateral motion trajectory. I’m planning on testing these new coefficients later today or tomorrow after some other work is done, to compare them to the first coefficients, and to compare the subsequent estimates.

Within the net, I’m planning on accounting for the limited space by performing a flight where the drone takes off in altitude mode, pitches forward until sufficient velocity is reached, level it off and let it stop, repeat the same maneuver backwards, and repeat the entire process several times for that flight. After obtaining the log file for that flight, I plan on doing the same, but with the drone moving laterally. I hope that I can get relatively good coefficients for each direction by using the individual test files, but I will update on the efficacy of that approach when I complete the process.

One other issue with the real drone has been that using the initially obtained coefficients with all of the known wind estimation parameters turned on did not enable the wind estimate states in columns 23/24 of the ekf state estimate output. This could be caused by very low wind disturbances not being picked up, but further testing is required to know for sure, unless you know what could be preventing it from populating.

Thank you again!

It’s probably a stupid question, but did you enable “multirotor drag fusion” im EKF2_AID_MASK? And do you have a valid velocity estimate?

It’s not stupid, it took me a while to find that parameter for the Gazebo simulations. The following parameters were changed in the process of trying to get it turned on, including the aid mask:

  • EKF2_AID_MASK → Multi-rotor drag fusion
  • SDLOG_PROFILE → Estimator Replay EKF2
  • EKF2_ARSP_THR → Set to ~0.1
  • EKF2_FUSE_BETA → Enabled
  • CAL_AIR_CMODEL → Model w/o pitot

I’m not sure if there was a valid velocity estimate or not, but states 4:6 of what I’ve been assuming to be the ekf states in the ulog topic “estimator_states” does populate with values in from the log file of the real drone flight. Could one of the other settings I changed be affecting the estimate?

Do you have any horizontal position/velocity sensors? (e.g.: gps, optical flow, vio, …)

The vehicle in question does have an onboard gps, but I’m unsure if it was turned off temporarily to meet another project’s goals. We will try again and double check that gps is turned on.

Thank you!

Hello @bresch

Maybe I’m missing some details, but the quadratic term in the script:
(Edit: I means 0.5 * rho * v_body[0] ** 2 * np.sign(v_body[0]) * x[1], sorry for confusion)

seems inconsistent with the publication.


V0 is the relative air velocity in the inertia frame.

In the script the (parasite?) drag is related to the square of speed component (v_body[dim] ** 2)
but the paper said it should be related to v_body[dim] * v_body.norm().

So here are my questions:

  1. Is this inconsistency a bug?
  2. If I want to estimate the aerodynamic drag based on the true airspeed, which formula should I use?

If I’m not mistaken, in mc_wind_estimator_tuning.py, the term in the red box is directly found from the equation you posted from the paper (boxed in red in my original question) and a simplified body drag equation, found as follows:

image

Which I believe is roughly equivalent to distributing the V0 in the denominator of the equation in your screenshot to the right side, then solving for acceleration.

If you do the same thing shown above but without solving for acceleration (distributing the mass term), you should get the aerodynamic drag. After trying the mc_wind_estimator_tuning.py with and without the momentum drag term (boxed in red in my original question), the second drag acceleration term does roughly estimate the general shape of acceleration, but the momentum drag term really locks in the estimate. I haven’t tried the tuning without that drag acceleration term, but here’s some screenshots of trying it with and without the momentum drag term using gazebo simulations, in just one direction.

With both terms:

With just the drag term, no momentum coefficient:

I could be wrong, but this is at least how I understand it.

1 Like

Hi Nick!
Thanks for sharing your thought! :smile: I’m new to the aerodynamic so I’m happy to have someone to discuss about it.

What I feel weird about is the second term (at the right side of red box, which is also presented in your image). Specifically, it’s the choice to distribute the V0 (the relative velocity, which is a vector) component before or after the drag force calculation.

If my understanding is correct, the “v” in the drag equation should be the relative airspeed (v = ||V0||, the length of V0, a scalar, NOT a vector). So to obtain the drag force in each axis, it seems like we should calculate the total body drag first before distribute it to each axis.

Since the body drag force (vector) has same direction with V0, the relative velocity to the air. Decomposing the drag acceleration along 3-axis (says the x axis) will get:
image

Because the norm (length) of a vector can greater than its component, the drag equation in the script may no longer hold if both axis has air flow simultaneously (wind from Northeast, for example).

You’re right. However I’ve never seen this in any textbook so we simply used the usual scalar drag equation for each axis where the body drag is proportional to the dynamic pressure, which is proportional to v^2.

@bresch Yes, I get it.
The question is, we cannot simply replace the v with the velocity components at each axis since the drag is proportional to v^2. Or else the product of the drag force at each axis will not consistent with the total drag force applied to the inertia frame.

Note: Because this topic is already solved, I create a new topic for this separated issue.
:laughing: We can have further discussion here: