function

Control.Multirotor.LogLinear.integratePositionError

Advance and symmetrically bound the world-frame position-error integral

Applies a forward-Euler step to world-frame position error. Keeping the memory in a fixed frame prevents a yaw manoeuvre from rotating an accumulated disturbance estimate. Each axis is limited so its acceleration contribution cannot exceed the configured fraction of gravity.

Public declarations

Explicit source declarations; dimensions, modifications and defaults are shown as written, without evaluation.

Role	Name	Type	Dimensions / attributes / default	Description
input	`positionErrorWorld`	`Real`	`[3](each unit = "m")`
input	`positionIntegralWorld`	`Real`	`[3]`	Current world-frame integral state
input	`dt`	`Real`	`(unit = "s")`	Integration interval
input	`gravity`	`Real`	`(unit = "m/s2")`	Positive gravitational acceleration
input	`integralGain`	`Real`	`[3] = {0.05, 0.05, 0.05}`
input	`integralFractionLimit`	`Real`	`[3] = {0.25, 0.25, 0.25}`	Maximum integral acceleration as a fraction of gravity
output	`nextPositionIntegralWorld`	`Real`	`[3]`	Bounded next world-frame integral state

Modelica declarations and implementation

Show parameters, interfaces and source

Parameters, inputs, outputs, defaults and equations from the source file.

function integratePositionError
    "Advance and symmetrically bound the world-frame position-error integral"
    input Real positionErrorWorld[3](each unit = "m");
    input Real positionIntegralWorld[3]
      "Current world-frame integral state";
    input Real dt(unit = "s") "Integration interval";
    input Real gravity(unit = "m/s2") "Positive gravitational acceleration";
    input Real integralGain[3] = {0.05, 0.05, 0.05};
    input Real integralFractionLimit[3] = {0.25, 0.25, 0.25}
      "Maximum integral acceleration as a fraction of gravity";
    output Real nextPositionIntegralWorld[3]
      "Bounded next world-frame integral state";
  protected
    Real limit;
  algorithm
    assert(dt >= 0.0, "Controller time step must be nonnegative");
    assert(gravity > 0.0, "Gravity must be positive");
    for axis in 1:3 loop
      assert(integralGain[axis] > 0.0,
        "Every position integral gain must be positive");
      assert(integralFractionLimit[axis] >= 0.0,
        "Every position integral limit must be nonnegative");
      limit := gravity * integralFractionLimit[axis] / integralGain[axis];
      nextPositionIntegralWorld[axis] := MathUtilities.clip(
        positionIntegralWorld[axis] + dt * positionErrorWorld[axis],
        -limit,
        limit);
    end for;
    annotation(Documentation(info="<html>
      <p>Applies a forward-Euler step to world-frame position error. Keeping
      the memory in a fixed frame prevents a yaw manoeuvre from rotating an
      accumulated disturbance estimate. Each axis is limited so its
      acceleration contribution cannot exceed the configured fraction of
      gravity.</p>
    </html>"));
  end integratePositionError;