[sfwa]

Flight dynamics model, part 2

Having developed a model based on a simulation of the airframe in XFLR5, the next step was to incorporate throttle and control surface deflection for increased accuracy. Since we had trouble modelling elevons, we decided to use the X8 we’d already developed in X-Plane, and log data from the X-Plane flight dynamics model to compare against our own model. Ultimately the settings would be tested against the data we recorded during test flight 3, with the results shown here:

Although the lift and drag forces from the XLFR5-derived coefficients agreed fairly well with the X-Plane data, the side force and yaw/pitch/roll moments were significantly different, even with no control surface input.

The main reason for this is that in conditions involving no angular acceleration (i.e. almost all the time), the moment due to control surface deflection and moment due to angular velocity, while individually quite large, sum to zero. Thus, the coefficients have to be closely matched in order for the model to yield realistic angular velocities.

So, to facilitate better tuning of the model, we simplified the calculation of forces and moments (with the exception of lift and drag, which were already working well):

The resulting model code was also simpler; on a Core i7 the model evaluation takes 450 cycles, and the DSP should perform similarly.

AccelerationVector FixedWingFlightDynamicsModel::evaluate(
const State &in, const ControlVector &control) const {
    /* Cache state data for convenience */
    Quaternionr attitude = Quaternionr(in.attitude());
    real_t yaw_rate = in.angular_velocity()[2],
           pitch_rate = in.angular_velocity()[1],
           roll_rate = in.angular_velocity()[0];

    /* External axes */
    Vector3r airflow, lift_axis, drag_axis, side_axis;
    real_t v, v_inv;

    airflow = attitude * (in.wind_velocity() - in.velocity());
    v = airflow.norm();

    if (v < DYNAMICS_MIN_V || v > DYNAMICS_MAX_V ||
            std::abs(yaw_rate) > DYNAMICS_MAX_RATE ||
            std::abs(roll_rate) > DYNAMICS_MAX_RATE ||
            std::abs(pitch_rate) > DYNAMICS_MAX_RATE) {
        /* Airflow too slow or too fast for any of this to work */
        return AccelerationVector();
    }

    v_inv = 1.0 / v;

    /*
    Lift is always perpendicular to airflow, drag is always parallel, and
    side is always towards the starboard wing of the aircraft.
    */
    drag_axis = airflow * v_inv;
    lift_axis = drag_axis.cross(Vector3r(0, 1, 0));
    side_axis = drag_axis.cross(lift_axis);

    /* Determine alpha and beta: alpha = atan(wz/wx), beta = atan(wy/|wxz|) */
    real_t alpha, beta, qbar, alpha2, beta2;
    qbar = RHO * v * v * 0.5;
    alpha = std::atan2(-airflow.z(), -airflow.x());
    beta = std::asin(airflow.y() * v_inv);

    if (std::abs(alpha) > M_PI / 2.0 || std::abs(beta) > M_PI / 2.0) {
        /* Alpha or beta completely out of range */
        return AccelerationVector();
    }

    alpha2 = alpha * alpha;
    beta2 = beta * beta;

    /*
    Set up a^4, a^3, a^2, a, 1 so that we can use dot product for the
    polynomial evaluation.
    */
    Eigen::Matrix<real_t, 5, 1> alpha_coeffs;
    alpha_coeffs << alpha2 * alpha2, alpha2 * alpha, alpha2, alpha, 1.0;

    /* Evaluate quartics in alpha to determine lift and drag */
    real_t lift = alpha_coeffs.dot(c_lift_alpha),
           drag = alpha_coeffs.dot(c_drag_alpha);

    /*
    Determine motor thrust and torque:
    https://www.grc.nasa.gov/WWW/Wright/airplane/propth.html

    Propeller thrust =
    F = 0.5 * rho * A * (Ve^2 - V0^2)
    where A = propeller disc area, Ve = exit velocity, V0 = air velocity

    In this formulation,
    F = 0.5 * rho * A * ((k * rpm)^2 - V0^2)

    Presumably the relationship between thrust and torque on airframe is
    constant? If so, they're related by prop_ct.
    */
    real_t thrust = 0.0;
    if (motor_idx < control.rows()) {
        real_t ve = prop_cve * control[motor_idx], v0 = airflow.x();
        thrust = 0.5 * RHO * prop_area * (ve * ve - v0 * v0);
        if (thrust < 0.0) {
            /* Folding prop, so assume no drag */
            thrust = 0.0;
        }
    }

    /*
    Set up the side force coefficient vector, and calculate side force
    */
    Eigen::Matrix<real_t, 8 + CONTROL_MAX_DIM, 1> side_coeffs;
    side_coeffs.segment<8>(0) << alpha2, alpha, beta2, beta,
                                 alpha2 * beta, alpha * beta,
                                 yaw_rate, roll_rate;
    side_coeffs.segment<CONTROL_MAX_DIM>(8) << control;

    real_t side_force = side_coeffs.dot(c_side_force);

    /*
    Calculate pitch moment
    */
    Eigen::Matrix<real_t, 2 + CONTROL_MAX_DIM, 1> pitch_coeffs;
    pitch_coeffs.segment<2>(0) <<
        alpha, pitch_rate * pitch_rate * (pitch_rate < 0.0 ? -1.0 : 1.0);
    pitch_coeffs.segment<CONTROL_MAX_DIM>(2) << control;

    real_t pitch_moment = pitch_coeffs.dot(c_pitch_moment);

    /*
    Roll moment
    */
    Eigen::Matrix<real_t, 1 + CONTROL_MAX_DIM, 1> roll_coeffs;
    roll_coeffs[0] = roll_rate;
    roll_coeffs.segment<CONTROL_MAX_DIM>(1) << control;

    real_t roll_moment = roll_coeffs.dot(c_roll_moment);

    /*
    Yaw moment
    */
    Eigen::Matrix<real_t, 2 + CONTROL_MAX_DIM, 1> yaw_coeffs;
    yaw_coeffs.segment<2>(0) << beta, yaw_rate;
    yaw_coeffs.segment<CONTROL_MAX_DIM>(2) << control;

    real_t yaw_moment = yaw_coeffs.dot(c_yaw_moment);

    /*
    Sum and apply forces and moments
    */
    Vector3r sum_force, sum_torque;

    sum_force = qbar * (lift * lift_axis + drag * drag_axis +
                side_force * side_axis) + thrust * motor_thrust;
    sum_torque = qbar * Vector3r(roll_moment, pitch_moment, yaw_moment);

    /* Calculate linear acceleration (F / m) */
    AccelerationVector output;
    output.segment<3>(0) = sum_force * mass_inv +
                           attitude * Vector3r(0, 0, G_ACCEL);

    /* Calculate angular acceleration (tau / inertia tensor) */
    output.segment<3>(3) = inertia_tensor_inv * sum_torque;

    return output;
}

We used scipy’s optimize.minimize() function to fit the coefficients to the X-Plane data log, and then hand-tuned the values that didn’t optimise well (in particular, pitch and roll moment).

Compared with the previous models (results benchmarked against the same video from test flight 3, so the minimum possible error is probably in the 1–2° range, but the baseline errors are consistent between models):

Model ° Error at Percentile
Pitch Roll
50th 75th 95th RMS 50th 75th 95th RMS
None 4.0 6.6 13.4 6.5 4.5 7.8 14.6 7.3
Centripetal 2.6 4.4 9.3 5.4 3.0 5.4 12.8 7.7
Fixed-wing 1 2.2 3.7 7.2 4.3 2.7 4.7 8.3 6.1
Fixed-wing 2 2.4 3.8 6.4 3.4 2.2 3.7 6.1 3.3

Taking a sample from test flight 3, with attitude based on detected horizon in black, and attitude from the UKF with fixed-wing dynamics model 2 in red (top chart is pitch, bottom chart is roll):

Plot of pitch from detected horizon (black) compared against pitch from UKF (red), with fixed-wing dynamics model 2 enabled Plot of roll from detected horizon (black) compared against roll from UKF (red), with fixed-wing dynamics model 2 enabled

Based on the noise in the pitch values from the horizon detection program, it looks like there’s more error in detected pitch than detected roll—makes some sense considering the aspect ratio of the video, and the fact that roll is largely unaffected by whatever residual distortion remains after OpenCV and perspective correction. Still, it shows the filter tracks reasonably well through a significant roll range, e.g. around frame 70,000 where roll goes from –100° to 100° over the course of seven seconds (1680 video frames, 7000 UKF iterations).

github.com/sfwa twitter.com/sfwa_uav youtube.com/user/sfwavideo