# Acceleration Control Maths

**URL:** <https://discuss.px4.io/t/acceleration-control-maths/26475>\
**Category:** PX4 Autopilot\
**Created:** [March 6, 2022, 6:41pm UTC](https://discuss.px4.io/t/acceleration-control-maths/26475 "2022-03-06T18:41:58Z")\
**Posts on this page:** 5\
**Page:** 1

<div class="post-metadata">

**Author:** ![Pattius](https://discuss.px4.io/user_avatar/discuss.px4.io/pattius/32/9325_2.png) [@Pattius](https://discuss.px4.io/u/Pattius)\
**Post date:** [March 6, 2022, 6:41pm UTC](https://discuss.px4.io/t/acceleration-control-maths/26475/1 "2022-03-06T18:41:58Z")

</div>

Hi,

I’m trying to figure out the purpose and maths behind the \_accelerationControl() function (code below) within the Position Control code (PositionControl.cpp).

From what I can tell the purpose of the function is to convert the acceleration setpoints output by the Velocity controller into thrust setpoint values which are normalised to the hover thrust.

I would have thought that the simplest way to do this would be to add CONSTANTS\_ONE\_G to the z component of the acceleration setpoint and then to scale the acceleration setpoints by \_hover\_thrust / CONSTANTS\_ONE\_G.

The maths for the Z setpoint does work out like this but the setpoints for X & Y are set by a slightly different scale factor (which is related to the collective\_thrust variable) which is confusing me.

Can anyone shed light on the maths for me? Is there a paper somewhere that is is based on? Maybe I’m getting the purpose of the function slightly wrong. Any information or resources that could point me in the right direction would be very much appreciated!

> accelerationControl  
> void PositionControl::\_accelerationControl()  
> {  
> // Assume standard acceleration due to gravity in vertical direction for attitude generation  
> Vector3f body\_z = Vector3f(-\_acc\_sp(0), -\_acc\_sp(1), CONSTANTS\_ONE\_G).normalized();  
> ControlMath::limitTilt(body\_z, Vector3f(0, 0, 1), \_constraints.tilt);  
> // Scale thrust assuming hover thrust produces standard gravity  
> float collective\_thrust = \_acc\_sp(2) \* (\_hover\_thrust / CONSTANTS\_ONE\_G) - \_hover\_thrust;  
> // Project thrust to planned body attitude  
> collective\_thrust /= (Vector3f(0, 0, 1).dot(body\_z));  
> collective\_thrust = math::min(collective\_thrust, -\_lim\_thr\_min);  
> \_thr\_sp = body\_z \* collective\_thrust;  
> }

---

<div class="post-metadata">

**Author:** ![robwaat](https://discuss.px4.io/letter_avatar_proxy/v4/letter/r/50afbb/32.png) [@robwaat](https://discuss.px4.io/u/robwaat)\
**Post date:** [March 9, 2022, 12:41pm UTC](https://discuss.px4.io/t/acceleration-control-maths/26475/2 "2022-03-09T12:41:58Z")

</div>

Hi Pattius,

The [Px4 controller documentation](https://docs.px4.io/master/en/flight_stack/controller_diagrams.html) points to [this report](https://www.research-collection.ethz.ch/bitstream/handle/20.500.11850/154099/eth-7387-01.pdf) as the basis for the attitude controller, but I’m not sure that covers your question very well.

I agree with your assessment of the function’s purpose. The `_accelerationControl()` function is responsible for converting the desired acceleration vector to the desired normalised thrust vector. This is conducted in a world aligned Front-Right-Down (FRD) xyz coordinate frame, which is standard for aerospace systems, and might explain some of the confusion.  
It also limits the normalised thrust setpoint to account for the maximum tilt the drone can safely reach while remaining airborne. I think this is might be why the XY components are initially handled seperately to the Z setpoint, rather than combining them as you suggest.

Looking at the code, I would agree that the scaling factor of the `_thr_sp` x-y components will be slightly off versus the desired acceleration. Use of the modified `collective_thrust` to scale these components introduces effects due to the `- hover_thrust` term and its scaling to prioritise the `_thr_sp` z-component. The tilit limits will also have an effect, though this one is desired.  
That said, I’d expect that the `_thr_sp` x-y components will still be roughly proportional to the desired value and point in the correct direction, allowing the larger controller structure to correct for the difference at the position and velocity control levels, so I don’t know how much of a difference this makes to flight performance.

It might make sense to look into the `ControlMath::limitTilt()` function and a simulation to compare your suggested approach with what already exists.

Hope this helps,  
Robert.

# Code Explanation

As far as the maths of this function goes, my understanding of each of the line of the function is:

```cpp
// Assume standard acceleration due to gravity in vertical direction for attitude generation
Vector3f body_z = Vector3f(-_acc_sp(0), -_acc_sp(1), CONSTANTS_ONE_G).normalized();

```

This line computes an initial value for the unit vector orientation of the drone’s body frame z axis, expressed in the world aligned FRD frame `{W}`. The drone thrust is going to act in the negative body z-axis so, to provide the desired acceleration in the `{W}` xy-axes, the body z-axis must point in the opposite direction to the acceleration setpoint. To hover, the drone will also be providing thust upwards to give acceleration of negative `CONSTANTS_ONE_G` in the `{W}` z-axis, so the body z-axis must again have this component in the opposite direction.  
Normalising this converts it to a unit vector expression of orientation. The rest of the code then caluclates a scaling factor to give the thrust in normalised units.

```cpp
ControlMath::limitTilt(body_z, Vector3f(0, 0, 1), _constraints.tilt);

```

This function alters the x-y components of the unit body\_z vector to ensure the drone does not exceed the required tilt angle. It modifies `body_z` in place, returning a re-normalised unit vector that obeys the tilt limits.

```cpp
// Scale thrust assuming hover thrust produces standard gravity
float collective_thrust = _acc_sp(2) * (_hover_thrust / CONSTANTS_ONE_G) - _hover_thrust;

```

This line gives the required normalised thrust setpoint in the `{W}` z-axis. Again, the result will likely be negative due to the FRD frame.  
The first term gives the thrust required to meet the z-axis acceleration setpoint. Essentially the `(_hover_thrust / CONSTANTS_ONE_G)` is a unit conversion factor. I’d expect its units to be something like `norm_thrusts/(m/s^2)`, calculated with `_horver_thrust` being in the range `[0,1] norm_thrusts` and `CONSTANTS_ONE_G` being `9.81 m/s^2`.  
The second term adds `- _hover_thrust` to account for thrust required to hover with drone weight acting in the positive `{W}` z-axis.

```cpp
// Project thrust to planned body attitude
collective_thrust /= (Vector3f(0, 0, 1).dot(body_z));

```

This divides the collective thrust by the component of the `body_z` vector acting in the `{W}` z-axis. This scaling step means that the `_thr_sp` z-componet will have the magnitude of `collective_thrust` calculated in the line above when the two are multiplied together later.

```cpp
collective_thrust = math::min(collective_thrust, -_lim_thr_min);

```

This limits the thrust scale to the minimum output that can be produced without stopping the propellers or spinning them in reverse. It prevents excessive downwards acceleration.

```cpp
_thr_sp = body_z * collective_thrust;

```

The magnitude of the desired, re-scaled `collective_thrust` is applied to the `body_z` orientation to give the final thrust setpoint. The negative value of `collective_thrust` also flips the `body_z` direction giving the correct thrust in the `{W}` FRD coordinate frame.

---

<div class="post-metadata">

**Author:** ![Pattius](https://discuss.px4.io/user_avatar/discuss.px4.io/pattius/32/9325_2.png) [@Pattius](https://discuss.px4.io/u/Pattius)\
**Post date:** [March 9, 2022, 6:42pm UTC](https://discuss.px4.io/t/acceleration-control-maths/26475/3 "2022-03-09T18:42:45Z")

</div>

Hi Robert,

Thanks for such a good and thorough answer! Definitely clears things up a bit for me and will be a great reference for anyone else looking to understand this function.

I guess I was getting hung up on the fact the XY thrust scale factor is off a bit too much, as you said this will just get absorbed by the rest of the controller. There’s separate tuning parameters for XY & Z on the velocity controller which I imagine would counteract any discrepancies. I suppose it just wasn’t worth an additional line of code/ extra variables to correct the scale factor.

I was curious to see what difference it would actually make so I wrote a little python script (code below) that works out outputs of the acceleration function using both methods.

Method 1: Original PX4 method described in Roberts Post above.  
Method 2: Scaling adding gravity to the acceleration demands and scaling everything by Hover\_Thrust/Gravity.

Some results:  
Input: Acceleration Setpoint [2,2,0] Hover Thrust = 0.5  
Output: Method 1 [0.1019368 0.1019368 -0.5]  
Method 2 [0.1019368 0.1019368 -0.5]

Input: Acceleration Setpoint [2,2,-3] Hover Thrust = 0.5  
Output: Method 1 [0.13311013 0.13311013 -0.6529052]  
Method 2 [0.1019368 0.1019368 -0.6529052]

As you can see the the XY acceleration output gets scaled differently proportional to how much Z acceleration is demanded (as a consequence of scaling the whole output by the collective\_thrust term). Still seems a bit odd to me but willing to accept that it probably makes no difference!

import numpy as np  
import math  
def main():  
a\_sp = np.array([2,2,-3])  
g = 9.81  
Th = 0.5  
acc\_control\_px4(a\_sp,g,Th)  
acc\_control\_alt(a\_sp,g,Th)

```
def acc_control_px4(a_sp,g,Th):

    body_z = np.array([-a_sp[0],-a_sp[1],g])
    body_z = body_z/np.sqrt(body_z.dot(body_z))
    body_z = limitTilt(body_z,np.array([0,0,1]), 0.5)

    collective_thrust = a_sp[2]*Th/g-Th
    collective_thrust /= np.array([0,0,1]).dot(body_z)

    thrust_sp = body_z * collective_thrust

    print("Orignal PX4 Output" ,thrust_sp)

def acc_control_alt(a_sp,g,Th):
    SF = g/Th
    A_ac = a_sp + np.array([0,0,-g])
    thrust_sp = A_ac/SF

    print("Alternative Output", thrust_sp)

def limitTilt(body_unit, world_unit, max_angle):

    # determine tilt
    dot_product_unit = body_unit.dot(world_unit)
    angle = math.acos(dot_product_unit)
    # limit tilt
    angle = np.minimum(angle, max_angle)
    rejection = body_unit - (dot_product_unit * world_unit)
    rejection_unit = rejection/np.sqrt(rejection.dot(rejection))
    return math.cos(angle) * world_unit + math.sin(angle) * rejection_unit

main()
```

---

<div class="post-metadata">

**Author:** ![Sunlight](https://discuss.px4.io/user_avatar/discuss.px4.io/sunlight/32/13004_2.png) [@Sunlight](https://discuss.px4.io/u/Sunlight)\
**Post date:** [April 26, 2023, 6:30am UTC](https://discuss.px4.io/t/acceleration-control-maths/26475/4 "2023-04-26T06:30:04Z")

</div>

Hi,  
Thanks for your excellent explanation for the codes, but in the code for getting the `body_z` orientation, why not use the `-acc_sp(2)+CONSTANTS_ONE_G` as input but use `CONSTANTS_ONE_G` only? If for gravity acceleration g, the ` -acc_sp(2)` is too small so it can be ignored, it really confuse of me.

`Vector3f body_z = Vector3f(-_acc_sp(0), -_acc_sp(1), -_acc_sp(2)+CONSTANTS_ONE_G).normalized();`

Hope this helps.

---

<div class="post-metadata">

**Author:** ![Sunlight](https://discuss.px4.io/user_avatar/discuss.px4.io/sunlight/32/13004_2.png) [@Sunlight](https://discuss.px4.io/u/Sunlight)\
**Post date:** [April 26, 2023, 7:22am UTC](https://discuss.px4.io/t/acceleration-control-maths/26475/5 "2023-04-26T07:22:21Z")

</div>

I have found the explanation for it 🙂：

> <https://github.com/PX4/PX4-Autopilot/issues/19821>
>
> During the calculation from desired acceleration setpoint to desired body\_z dire…ction, it is done by the following code:
> 
> \`\`\`
> void PositionControl::\_accelerationControl()
> {
> // Assume standard acceleration due to gravity in vertical direction for attitude generation
> Vector3f body\_z = Vector3f(-\_acc\_sp(0), -\_acc\_sp(1), CONSTANTS\_ONE\_G).normalized();
> ControlMath::limitTilt(body\_z, Vector3f(0, 0, 1), \_lim\_tilt);
> // Scale thrust assuming hover thrust produces standard gravity
> float collective\_thrust = \_acc\_sp(2) \* (\_hover\_thrust / CONSTANTS\_ONE\_G) - \_hover\_thrust;
> // Project thrust to planned body attitude
> collective\_thrust /= (Vector3f(0, 0, 1).dot(body\_z));
> collective\_thrust = math::min(collective\_thrust, -\_lim\_thr\_min);
> \_thr\_sp = body\_z \* collective\_thrust;
> }
> \`\`\`
> It uses a "CONSTANTS\_ONE\_G" and ignores the \_acc\_sp(2) when calculating body\_z.
> May I know what is the advantage of doing this? @MaEtUgR
> And is it possible to add a mode that also considers \_acc\_sp(2) when calculating body\_z?