Tests.PositionLoopStabilityTests.routhCoefficients

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

function routhCoefficients
    "Coefficients of s^3 + a2 s^2 + a1 s + a0 for the closed east loop"
    output Real coefficient[3] "a2, a1, a0";
  protected
    // RDD2 parameterization, matching Vehicles.Rdd2.LogLinearController.
    constant Real mass = 2.0;
    constant Real gravity = 9.8;
    constant Real thrustTrim = 19.6;
    constant Real attitudeGain[3] = {2.0, 2.0, 1.0};
    constant Real step = 1.0e-4;
    Real positionSensitivity;
    Real velocitySensitivity;
    Real pitchSensitivity;
  algorithm
    // Numerically linearize the shipped control law about hover. The plant
    // rows are exact: d(position)/dt = velocity, and d(velocity)/dt =
    // gravity * pitch for a thrust-vectored vehicle holding hover thrust.
    positionSensitivity := (
      commandedPitchRate(step, 0.0, 0.0, mass, gravity, thrustTrim,
        attitudeGain)
      - commandedPitchRate(-step, 0.0, 0.0, mass, gravity, thrustTrim,
        attitudeGain)) / (2.0 * step);
    velocitySensitivity := (
      commandedPitchRate(0.0, step, 0.0, mass, gravity, thrustTrim,
        attitudeGain)
      - commandedPitchRate(0.0, -step, 0.0, mass, gravity, thrustTrim,
        attitudeGain)) / (2.0 * step);
    pitchSensitivity := (
      commandedPitchRate(0.0, 0.0, step, mass, gravity, thrustTrim,
        attitudeGain)
      - commandedPitchRate(0.0, 0.0, -step, mass, gravity, thrustTrim,
        attitudeGain)) / (2.0 * step);

    // Characteristic polynomial of the closed loop
    //   [0 1 0; 0 0 g; positionSensitivity velocitySensitivity pitchSensitivity]
    coefficient[1] := -pitchSensitivity;
    coefficient[2] := -gravity * velocitySensitivity;
    coefficient[3] := -gravity * positionSensitivity;
  end routhCoefficients;